Apparatus for an automated aerial refueling boom using multiple types of sensors

ABSTRACT

A system for automated control of a refueling boom coupled to a tanker aircraft is provided. The system includes: a first inertial measurement unit (IMU) providing inertial measurements for the tanker aircraft; a first GPS receiver providing a GPS location for a GPS antenna attached to the tanker aircraft; and a processor adapted to calculate a first inertial navigation state for the tanker aircraft through integration of the inertial measurements, the processor being further adapted to calculate a first inertial navigation state error relative to the GPS location and to filter the first inertial navigation state error and the first inertial navigation state based upon noise characteristics of the first IMU and the first GPS receiver to provide an updated inertial navigation state for the tanker aircraft, the processor being further adapted to control the refueling boom relative to a receiver aircraft based upon the first and updated inertial navigation states.

RELATED APPLICATION

This application is related to U.S. patent application No. entitled“Fault Detection And Reconfiguration Of An Automated Refueling Boom,”concurrently filed herewith.

TECHNICAL FIELD

The present invention relates generally to the control of aerialrefueling booms, and more particularly to automated control of aerialrefueling booms using multiple sensors.

BACKGROUND

Aerial tankers use refueling booms to fuel other aircraft in mid-flight.In this fashion, the refueled (receiver) aircraft can fly extendedmissions during military or civilian operations. Generally, the aerialtanker and the receiver aircraft must be flown in close proximity toaccomplish a refueling. Given this proximity, a manual operator in theaerial tanker may extend its refueling boom to mate with the fuel tankof the receiver aircraft.

In general, the refueling boom operation can tax the skill of evenexperienced boom operators. The possibility of midair accidents is everpresent. In addition, the refueling boom may improperly mate with thereceiver aircraft. For example, a stealth aircraft may have its lowobservable coating contacted such that it becomes observable to enemyradar. In such a case, the stealth aircraft may have to abort itsmission and return for repair.

During the refueling operation, the boom operator must manually directthe refueling boom to mate with the receiver aircraft. This manualdirection must be accomplished in varied lighting conditions fromextremely bright sun light to complete darkness or minimum visibilityweather conditions. Moreover, the manual direction may be furthercomplicated by turbulence. Be cause of this manual operation, refuelingoperations are lengthy as compared to machine-guided refueling. Inaddition, the need for human boom operators makes unmanned aerialtankers unfeasible. Finally, manual operation is inevitably prone toerror and mishap.

Conventional machine-guided boom controls typically use an electro-opticsensor to estimate the boom and receiver aircraft position. By removingthe need for human boom operators, unmanned aerial tankers can bedeveloped, which can lead to significant cost savings. For example, ithas been estimated that manned aircraft use 80% of their service life totrain aircrews. Conventional electro-optic boom automated boom controlthus advantageously lessens the need for human operators. However,although electro-optic sensors can be quite accurate, they are rangelimited.

Accordingly, there is a need in the art for more robust machine-guidedboom control systems.

SUMMARY

Apparatuses, systems, and methods are disclosed herein which provideautomated refueling boom control in a cost-effective manner for amultitude of commercial and military applications. Specifically,apparatuses, devices, and methods are disclosed herein that relate tothe integration of data from multiple sensors to provide more robust andaccurate automated control of refueling booms.

In accordance with an embodiment, a system for automated control of arefueling boom coupled to a tanker aircraft is provided. The systemincludes: a first inertial measurement unit (IMU) providing inertialmeasurements for the tanker aircraft; a first GPS receiver providing aGPS location for a GPS antenna attached to the tanker aircraft; and aprocessor adapted to calculate a first inertial navigation state for thetanker aircraft through integration of the inertial measurements, theprocessor being further adapted to calculate a first inertial navigationstate error relative to the GPS location and to filter the firstinertial navigation state error and the first inertial navigation statebased upon noise characteristics of the first IMU and the first GPSreceiver to provide an updated inertial navigation state for the tankeraircraft, the processor being further adapted to control the refuelingboom relative to a receiver aircraft based upon the first and updatedinertial navigation states.

In accordance with another embodiment, a system for control of arefueling boom coupled to a tanker aircraft is provided. The systemincludes: an IMU providing tanker inertial measurements for the tankeraircraft; an IMU providing receiver inertial measurement for a receiveraircraft; and means for controlling the refueling boom so as toautomatically mate with the receiver aircraft, wherein the meanscontrols refueling boom by filtering the tanker and receiver inertialmeasurements relative to measurements from a sensor selected from thegroup consisting of a GPS receiver and an EO sensor.

In accordance with another embodiment, a method of controlling arefueling boom relative to a tanker aircraft and a receiver aircraft isprovided. The method includes the acts of: deriving an inertialnavigation state for the tanker aircraft; deriving an inertialnavigation state for the receiver aircraft; comparing the inertialnavigation states to additional sensor measurements to derive aninertial navigation state error for the tanker aircraft and an inertialnavigation state error for the receiver aircraft; filtering the inertialnavigation states and the inertial navigation state errors to provide anupdated inertial navigation state for the receiver aircraft and anupdated inertial navigation state for the tanker aircraft; andcontrolling the refueling boom based upon the updated inertialnavigations states so as to automatically mate with the receiveraircraft.

The scope of the present invention is defined by the claims, which areincorporated into this section by reference. A more completeunderstanding of embodiments of the present invention will be affordedto those skilled in the art, as well as a realization of additionaladvantages thereof, by a consideration of the following detaileddescription. Reference will be made to the appended sheets of drawingsthat will first be described briefly.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1 shows tanker components for an automated refueling boom controlsystem in accordance with an embodiment of the present invention.

FIG. 2 shows an receiver aircraft components for an automated refuelingboom control system according to an embodiment of the present invention.

FIG. 3 illustrates a fault detection and reconfiguration process for theautomated refueling boom control system of FIGS. 1 and 2.

Embodiments of the present invention and their advantages are bestunderstood by referring to the detailed description that follows. Itshould be appreciated that like reference numerals are used to identifylike elements illustrated in one or more of the figures.

DETAILED DESCRIPTION

Reference will now be made in detail to one or more embodiments of theinvention. While the invention will be described with respect to theseembodiments, it should be understood that the invention is not limitedto any particular embodiment. On the contrary, the invention includesalternatives, modifications, and equivalents as may come within thespirit and scope of the appended claims. Furthermore, in the followingdescription, numerous specific details are set forth to provide athorough understanding of the invention. The invention may be practicedwithout some or all of these specific details. In other instances,well-known structures and principles of operation have not beendescribed in detail to avoid obscuring the invention.

To provide a robust automated control system for aerial refueling, datafrom multiple sensors are integrated in a process that may be denoted as“sensor fusion” to accurately position a refueling boom with regard toboth the aerial tanker and the receiver aircraft. In one embodiment,this integration uses inertial state information from inertialmeasurement units (IMUs) located on the aerial tanker and the receiveraircraft to propagate inertial navigation states for each aircraft. Eachinertial navigation state has its own independent error dynamics.However, a combined state space may be formed which depends upon theerror dynamics from each IMU. This combined state space provides aconvenient format for mixing in other sensor information such as GPSsensor information or electro-optic (EO) sensor information to correctthe inertial navigation state using a filter such as an extended Kalmanfilter. Because both navigational states are thereby corrected, theresulting extended Kalman filter may be referred to as a Global ExtendedKalman Filter (GEKF).

In one embodiment, the tanker, the refueling boom, and the receiveraircraft are all processed through the GEKF so as to estimate therelative position velocity, and attitude between the tanker and thereceiver aircraft, from the tanker to the boom, and ultimately from theboom tip to the receiver aircraft receptacle. The inertial stateconsists of the position, velocity, and attitude of each player in aninertial reference frame. It may also be expanded to include linearacceleration and angular rate. Each IMU provides measurements of linearacceleration and angular rate for its location. The tanker, therefueling boom, and the receiver aircraft (each IMU-tracked object maybe denoted as a “player” in the following discussion) may have their ownIMU measurements. For example, FIG. 1 illustrates tanker-basedcomponents of an exemplary automated refueling boom control system. Therefueling boom (not illustrated) may be controlled by a boom controlcomputer 100 that receives information from a GEKF 105. Positionalinformation for the receiver aircraft is derived from an IMU 110. Aswill be explained further herein, velocity and angular accelerationmeasurements from IMU 110 may be integrated using, for example, strapdown equations of motion to provide position and attitude (roll, pitchand yaw) information for the receiver aircraft. GEKF 105 updates orcorrects the strap-down-equations-of-motion-derived positionalinformation based upon measurements from other sensors such as from aGPS receiver 115. This position data will be denoted as “a priori”position data in the following discussion. Receiver 115 providesGPS-derived positional measurements based upon GPS signals received at aGPS antenna 120. For GEKF 105 to correct the a priori position data withthe GPS-derived measurements, the position of the antenna 120 may bederived from the a priori position data through appropriate translationbased upon a body frame lever arm 125 between IMU 110 and GPS antenna120. GEKF 105 receives position data from other players such as thereceiver aircraft (discussed with regard to FIG. 2) through an antenna140 and a corresponding data link 145. The a priori position data forthese other players with regard to IMU 110 may be corrected through GEKF105 using measurements from electro-optic (EO) sensors such as a camera130. To do so, the a priori position data is translated based upon abody frame lever arm 135 between IMU 110 and camera 130.

Turning now to FIG. 2, complementary components may be located on areceiver aircraft. A receiver aircraft IMU 200 provides measurements toa GEKF 205 so that a priori position data may be derived as discussedwith regard to FIG. 1. This a priori position data may be updated basedupon GPS measurements from a GPS receiver 210 having a GPS antenna 215.The a priori position data for the receiver aircraft is translated tothe GPS antenna location using a body frame lever arm 220 between IMU200 and GPS antenna 215. GEKF 205 also receives position data from otherplayers by receiving signals through a data link 230 as received by anantenna 225. Referring back to FIG. 1, camera 130 has a field of view150 that allows the imaging of reference points such as reference points1 through N on the receiver aircraft. In addition, camera 130 will imagesimilar reference points on the refueling boom (not illustrated). Eachof the receiver aircraft reference points has a known body frame leverarm between the receiver aircraft IMU 220 and the correspondingreference point. Thus, the receiver aircraft a priori position data maybe translated using the corresponding lever arm to each of the referencepoints so that GEKF 205 may update its position as will be explainedfurther herein.

It may thus be seen that to estimate the inertial states of and therelative states between each player, some combination of measurementsfrom Global Positioning System (GPS) receivers, Inertial MeasurementUnits (IMU's), wireless or wired communication, and Electro-Optic (EO)sensors are blended in a process denoted herein as “sensor fusion.” Thisprocess may be classified into two stages or phases. A first stage isreferred to as the propagation phase. In this phase, the inertial statesfor each player are formed. The following discussion will assume that aninertial navigation state will be determined with regard to IMUmeasurements. However, it will be appreciated that the sensor fusiontechniques discussed herein may be applied in systems without IMUs. Foreach player that has an IMU, the linear accelerations and angular ratemeasurements if available are utilized to aid in the propagation of theinertial navigation state for that vehicle. The acceleration and angularrate measurements are integrated in time using the strap down equationsof motion to obtain the a priori position data, which may also bedenoted as the inertial navigation states for each player. A secondphase consists of utilizing other sensor measurements to adjust theinertial navigation states of each player (as determined in the firstphase). For example, GPS measurements may be utilized to correct theabsolute position estimates of each state. Differential GPS measurementsprovide precise corrections of the relative position estimates. EOmeasurements provide precise correction to the relative position andattitude estimates. Finally, if IMU data is available, errors in the IMUsuch as bias errors, scale factor errors, and misalignment errors may becalibrated and removed in order to improve the propagation phase of theestimation process. The navigation states thus created by the multisensor system may then be provided to a conventional Boom Control Unit(BCU) which in turn commands the boom actuators to move the boom towardthe target, and into contact. Given this overview, the first phase ofsensor fusion will be discussed as follows.

Propagation Phase for the Inertial States

Each player in the methodology, whether boom, tanker, or receiver, isrepresented with an inertial navigation state. This state consists ofthe position, velocity, and attitude relative to an inertial referenceframe. In one embodiment, the inertial reference frame consists of a setof axes fixed in inertial space. For example, an Earth Centered EarthFixed (ECEF) coordinate frame and consists of an orthogonal set ofcoordinate axes emanating from the center of the earth. The positivez-axis points through the North Pole, the positive x-axis points throughthe equator at the Greewich Meridian, and the y-axis completes theright-hand coordinate frame. The ECEF moves relative to the inertialframe at a constant rotation rate (defined by the Earth's rotation). Thestate may be expanded to include the angular rate and accelerationrelative to this frame.

Given this definition of ECEF coordinates, a player's inertial state xmay be represented as

$\begin{matrix}{x = \begin{bmatrix}P^{E} \\V^{E} \\Q_{B}^{E}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 1} \right)\end{matrix}$

In this embodiment, the state x consists of a position P^(E) in the ECEFframe, a velocity V^(E) in the ECEF frame, and a quatemion Q_(B) ^(E)defining a body frame attitude of the vehicle relative to the ECEFframe. In such embodiment, the body frame is defined separately for eachplayer. For example, the body frame may be consistent with typical“aircraft coordinates” such that it consists of an orthogonal coordinateaxis frame with a positive x axis through the nose of the aircraft, apositive y-axis through the starboard side of the aircraft and a z axispositive down.

Note that IMU measurements consist of the measurement of accelerationand angular rates in the body frame of the IMU which may or may not beconsistent with the vehicle body frame. However the IMU is rigidlymounted to the vehicle and a constant rotation may be employed to relatethe IMU to the vehicle frame.

Referring back to FIGS. 1 and 2, IMUs 200 and 110 provide accelerationand angular rates. These measurements may be processed to obtain aderivative of the inertial navigation state x (Eq. 1), designated as x.This derivative is integrated to obtain the propagated state using, forexample, the strap down equations of motion:

$\begin{matrix}\begin{matrix}{\overset{.}{x} = \begin{bmatrix}{\overset{.}{P}}^{E} \\{\overset{.}{V}}^{E} \\{\overset{.}{Q}}_{B}^{E}\end{bmatrix}} \\{= \begin{bmatrix}V^{E} \\{{C_{B}^{E}a^{B}} - {\omega_{IE}^{E} \times \left( {\omega_{IE}^{E} \times P^{E}} \right)} - {2\omega_{IE}^{E} \times V^{E}} + g^{E}} \\{{\frac{1}{2}\left\lbrack {\Omega_{EB}^{B} \times} \right\rbrack}Q_{B}^{E}}\end{bmatrix}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 2} \right)\end{matrix}$

where a matrix C_(B) ^(E) is the rotation matrix calculated from thequaternion Q_(B) ^(E), a specific force vector a^(B) represents thelinear acceleration exhibited on the vehicle in the vehicle body frame,a gravity vector g^(E) is represented in the ECEF frame, an earthrotation rate ω_(IE) ^(E) represents the rotation of the ECEF framerelative to the inertial reference frame, a vector cross product betweentwo vectors a and b is represented as a×b, and a quaternion rate matrix[Ω_(EB) ^(B)×] is defined as:

$\begin{matrix}{{\left\lbrack {\Omega_{EB}^{B} \times} \right\rbrack = \begin{bmatrix}0 & {- \omega_{x}} & {- \omega_{y}} & {- \omega_{z}} \\\omega_{x} & 0 & \omega_{z} & {- \omega_{y}} \\\omega_{y} & {- \omega_{z}} & 0 & \omega_{x} \\\omega_{z} & \omega_{y} & {- \omega_{x}} & 0\end{bmatrix}}{\omega_{EB}^{B} = \begin{bmatrix}\omega_{x} \\\omega_{y} \\\omega_{z}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 3} \right)\end{matrix}$

where the angular velocity term ω_(EB) ^(B) represents the angularvelocity of the vehicle body frame relative to the ECEF framerepresented in the vehicle body frame.

The measurements from each IMU are the linear acceleration in the bodyframe a^(B) and the angular velocity of the body frame relative to theinertial frame, ω_(IB) ^(B). The following definition holds for theangular velocities:

ω_(IB) ^(B)=ω_(IE) ^(E)+ω_(EB) ^(B)  (Eq. 4)

Using these definitions, it is possible to integrate the strap downequations of motion using the IMU measurements, if available. If not,the angular velocity and acceleration can be estimated using thecorrections from the GEKF defined herein.

Error in the Propagated State

The strap down equations of motion (Eq. 2) are susceptible to errors inthe acceleration or angular rate data. The errors grow as a function oftime. This growth is predictable using a linearization. Thelinearization estimates the error growth over short periods of time. Anerror growth may be defined as:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\overset{.}{P}}^{E}} \\{\delta \; {\overset{.}{V}}^{E}} \\{\delta \; {\overset{.}{q}}_{B}^{\overset{\_}{B}}}\end{bmatrix} = \mspace{79mu} {{\begin{bmatrix}0 & I & 0 \\{G - \left\lbrack {\omega_{IE}^{E} \times} \right\rbrack^{2}} & {- {2\left\lbrack {\omega_{IE}^{E} \times} \right\rbrack}} & {{- 2}C_{B}^{E}a^{B}} \\0 & 0 & {- \left\lbrack {\omega_{IB}^{B} \times} \right\rbrack}\end{bmatrix}\left\lbrack \begin{matrix}{\delta \; P^{E}} \\{\delta \; V^{E}} \\{\delta \; q_{B}^{\overset{\_}{B}}}\end{matrix} \right\rbrack} + \left\lbrack \begin{matrix}0 \\{C_{B}^{E}w_{a}} \\w_{g}\end{matrix} \right\rbrack}} & \left( {{Eq}.\mspace{14mu} 5} \right)\end{matrix}$

where a gravity gradient G is a function of ECEF position defined by anEarth gravity model such as the J2 gravity model, a linearized quatemionerror q_(B) ^(E B) represents the linearized error in the fullquaternion Q_(B) ^(E), a noise term w_(a) represents the error in theaccelerometer measurements, a noise term w_(g) represents the error inthe angular rate measurements, and a quaternion error is defined as:

C _(B) ^(E) =C _(B) ^(E)(I+2[δq _(B) ^(B) ×])  (Eq. 6)

In this case C _(B) ^(E) is the a priori estimate of attitude calculatedfrom an a prior estimate Q _(B) ^(E) of Q_(B) ^(E). Eq. 6 also implies amethod of correcting the a priori rotation once the error is calculatedusing filtering methods derived below.

Note that Eq. (5) can be expanded to include other error sources. Forinstance, accelerometer bias, scale factor and misalignment may beincluded as additional states. In addition, the same errors may beincluded for the gyroscope measurements (if used). Finally, GPS clockerrors may be included. Adding other error sources further refines theerror model, thus improving knowledge of how errors in the navigationstate grow in time.

These dynamics are used in the Global Extended Kalman Filter (GEKF) asdiscussed further herein. Note that each player will have an inertialstate x as derived from integration of Eq. (2).

Each state is propagated using the available estimates of accelerationand angular velocity either from an IMU or from estimation using GPS/EO.The propagation of each state occurs independently of any other vehicle.The error growth of the state estimate is defined in Eq. (5).

These dynamics may be simplified to the form:

δ{dot over (x)}=Aδx+Bw  (Eq. 7)

Matrix A and vector B may be derived through a comparison of Eqs. (5)and (7). Note that noise terms (summarized as vector w) for the IMUmeasurements are known through, for example, a manufacturer-providedanalysis of each IMU. Thus, the unknown is the error term δx in the apriori position data/inertial navigation state x. Each player (such asthe tanker of FIG. 1 and the receiver aircraft of FIG. 2) has its ownindependent error term. However, a combined state space may be formedas:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\overset{.}{x}}_{1}} \\{\delta \; {\overset{.}{x}}_{2}}\end{bmatrix} = {{\begin{bmatrix}A_{1} & 0 \\0 & A_{2}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; x_{2}}\end{bmatrix}} + {\begin{bmatrix}B_{1} & 0 \\0 & B_{2}\end{bmatrix}\begin{bmatrix}w_{1} \\w_{2}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 8} \right)\end{matrix}$

where δx₁ is the error in the inertial navigation state of a firstvehicle and δx₂ is the error in the state of a second vehicle. A simplerotation can be used to redefine the error as:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\overset{.}{x}}_{1}} \\{\delta \; \Delta \; \overset{.}{x}}\end{bmatrix} = {{\begin{bmatrix}A_{1} & 0 \\{A_{1} - A_{2}} & A_{2}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{{\delta\Delta}\; x}\end{bmatrix}} + {\begin{bmatrix}B_{1} & 0 \\B_{1} & {- B_{2}}\end{bmatrix}\begin{bmatrix}w_{1} \\{\Delta \; w}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 9} \right)\end{matrix}$

where δΔx=δx₁−δx₂. This dynamic state is now in terms of the absolutenavigation error of the first vehicle and the relative navigation errorbetween the first vehicle and the second vehicle. The error shows howtwo inertial navigation states drift relative to each other. Thisrelative state is a convenient format for mixing measurements from, forexample, EO sensors and GPS sensors for correction of the inertialnavigation states. Each of these sensors may be provide a sensorposition estimate that may be compared to a IMU-derived positionestimate for the sensor (through appropriate translation) of the IMUinertial navigation state x. These other sensors also have a known noisebehavior as discussed with regard to the IMUs. A filter such as anExtended Kalman filter may then process the sensor position estimate,the IMU-derived sensor position estimate, and the noise behaviors forthe IMU and the sensor to determine the error term δx of Eq. (7). Theinertial navigation state x may then be corrected. In this fashion,although the sensor and IMU measurements are noisy, the inertialnavigation state is estimated having an error that is optimal in aleast-means-squared sense. Assuming that the combined error space fromEq. (9) is used, a resulting Extended Kalman filter may be referred toas a Global Extended Kalman Filter (GEKF) as shown in FIGS. 1 and 2because of the error terms for both players are determinedsimultaneously.

Note the advantages of such an automated boom control system. BecauseIMU measurements are fused with other sensor measurements in astatistically optimal fashion, very accurate automatic control of therefueling boom is made possible for the first time. In this fashion,unmanned aerial refueling tankers are enabled, providing significantcost savings for both commercial and military applications. The solutionis quite flexible in that multiple types of sensor measurements may befused together. Not only is a statistically optimal control solutionprovided, but fault detection methods are also disclosed. Should faultsbe detected in, for example, acceleration sensors in the IMU or GPSsatellite transmissions, an appropriate reconfiguration of the automatedrefueling boom control system is disclosed. Sensor fusion with regard toGPS measurements will be addressed first in the following discussion.

Correction of the Propagated Inertial States Using GPS Measurements

GPS measurements include measurements of a position and a velocity for aGPS receiver antenna. These measurements may be used to correct aninertial state provided that the lever arm between the GPS antenna andthe location of the inertial state reference point is known. Forinstance, the strap down equations of motion (Eq. 1.1.2) are typicallyintegrated at the location of the IMU. Transfer of the IMU positionestimates to the GPS antenna location is performed as follows:

P _(GPS) ^(E) =P _(IMU) ^(E) +C _(B) ^(E) L _(I2) G  (Eq. 10)

where P_(GPS) ^(E) is the position of the GPS antenna location in theECEF coordinate frame, P_(IMU) ^(E) is the position of the IMU in theECEF coordinate frame matrix, and L_(I2G) represents the vector distancefrom the IMU to the GPS antenna in the vehicle body frame. Velocityestimates may be transferred using similar methods:

V _(GPS) ^(E) =V _(IMU) ^(E) +C _(B) ^(E)(ω_(IB) ^(B) ×L _(I2) G)−ω_(Ie)^(E) ×C _(B) ^(E) L _(I2) G  (Eq. 11)

where V_(GPS) ^(E) is the velocity for the GPS antenna in the ECEFcoordinate frame, and V_(IMU) ^(E) is the velocity for the IMU in theECEF coordinate frame. Referring back to FIG. 1, an IMU-derived positionfor IMU 110 may thus be translated to provide an IMU-derived positionestimate for GPS antenna 120 based upon lever arm 125. For a given GPSantenna and an IMU inertial state, the GPS position measurements may bemodeled as a function of the inertial navigation state error using theperturbation methods defined previously with regard to Eq. (7). In thatregard, GPS receiver 115 provides a GPS-derived position estimate forGPS antenna 120 that may be denoted as {tilde over (P)}_(GPS) ^(E). Theinertial-navigation-state-derived position (which is typically anIMU-derived position) may be denoted as P _(GPS) ^(E). Using theinertial navigation state error term error term δx, {tilde over(P)}_(GPS) ^(E) may thus be defined using the following measurementfunction:

{tilde over (P)} _(GPS) ^(E) = P _(GPS) ^(E) +C _(P) δx+v  (Eq. 12)

where v is measurement noise, and a sensitivity matrix C_(P) is definedas:

C _(P) =[I0−2C _(B) ^(E) [L _(I2) G×]]  (Eq. 13)

The resulting cross product matrix in Eq. (13) operates against theattitude error in the inertial state error vector.

Note that more precise models may be incorporated. For instance, ifpseudorange measurements are utilized, then the state error dynamicsmust be augmented to include a clock error model and the positionmeasurements are augmented to include a clock bias. This example isincluded for brevity and clarity and is in no way limiting. Further, asimilar matrix may be constructed for the use of velocity measurementsor Doppler measurements from the GPS. If Doppler measurements areincluded, then the GPS clock bias error must be expanded to include aclock rate term and possibly a clock acceleration term.

Using the simple error model defined in Eq. (7) and combined with thedynamics defined in Eq. (12), a Global Extended Kalman Filter (GEKF) maybe utilized to estimate the state error δx as discussed further below.The state error may then be used to correct the a priori state estimateand improve the performance of the navigation solution. Note that GPSmeasurements are often available at a much lower rate than the IMU data.The IMU data is typically used to drive the strap down equations ofmotion. The error covariance of the GEKF is propagated at the same rate(or slower if computational limitations exist) as the IMU. When GPSmeasurements are available, the GPS is used to correct the IMU state.The state is then propagated forward in time using the IMU measurementsuntil the next GPS measurement is available.

Correction of the Propagated Inertial States Using Differential GPSMeasurements

Differential GPS involves the difference between two sets of GPSmeasurement from different GPS receiver in relatively close proximity.Because of the resulting elimination of common mode errors, differentialGPS is typically more accurate than conventional GPS. However, adisadvantage is that differential GPS measurements lose their absoluteposition information. Therefore a combination of absolute and relativeGPS measurements may be utilized.

Given two sets of GPS measurements, for example, from GPS receivers 115and 210, Eq. (12) takes on the following form:

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{P}}_{{GPS}\; 1}^{E} \\{\overset{\sim}{P}}_{{GPS}\; 2}^{E}\end{bmatrix} = {\begin{bmatrix}{\overset{\_}{P}}_{{GPS}\; 1}^{E} \\{\overset{\_}{P}}_{{GPS}\; 2}^{E}\end{bmatrix} + {\begin{bmatrix}C_{P\; 1} & 0 \\0 & C_{P\; 2}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; x_{2}}\end{bmatrix}} + \begin{bmatrix}b_{c} \\b_{c}\end{bmatrix} + \begin{bmatrix}v_{1} \\v_{2}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 14} \right)\end{matrix}$

Each of the measurements is independent of the other. However, they areboth corrupted by the same common mode errors b_(c). Using a rotationsimilar to the one employed Eq. (9), the relative state errors maymodified to the following form:

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{P}}_{{GPS}\; 1}^{E} \\{\Delta {\overset{\sim}{P}}_{{GPS}\;}^{E}}\end{bmatrix} = {\begin{bmatrix}{\overset{\_}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\_}{P}}_{GPS}^{E}}\end{bmatrix} + \mspace{160mu} {\begin{bmatrix}C_{P\; 1} & 0 \\{C_{P\; 1} - C_{P\; 2}} & C_{P\; 2}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{bmatrix}} + \begin{bmatrix}b_{c} \\0\end{bmatrix} + \begin{bmatrix}v_{1} \\{\Delta \; v}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 15} \right)\end{matrix}$

where a differential measurement is defined as Δ{tilde over (P)}_(GPS)^(E)={tilde over (P)}_(GPS1) ^(E)−{tilde over (P)}_(GPS2) ^(E). Therotation removes the effect of the common mode errors in the relativemeasurements. This new measurement function combined with the dynamicsof Eq. (9) can be used to define what is henceforward referred to as theGlobal Extended Kalman Filter (GEKF). The filter is referred to as aglobal filter since it incorporates the absolute and relative state ofboth vehicles and is corrected with the absolute and differential GPSmeasurements using the model in Eq. (15).

It will be appreciated that numerous modifications may be performed. Forinstance, raw pseudorange measurements from the GPS receiver may be usedinstead of position estimates. In this case, the state errors must beaugmented with a GPS clock bias (as discussed before) but also with arelative clock bias estimate. If velocity or Doppler measurements areemployed, then a state must be added for the clock drift and modeledappropriately. In addition, it is possible to process some or all of theavailable GPS measurements as they arrive. For instance, it is possibleto process the absolute measurements in the GEKF without thedifferential measurements until such time as the differential GPSmeasurements from both vehicles arrive.

Correction of the Propagated Inertial States Using GPS Carrier PhaseMeasurements

A very special subclass of measurements exists for GPS receiversequipped to provide carrier phase measurements because such measurementsmay be accurate to a few centimeters. However, to achieve this accuracy,an integer ambiguity problem must be solved. Once solved, thedifferential carrier phase measurements may be integrated into the GEKFand processed accordingly. The integer ambiguity problem arises becausean unknown integer number of wavelengths exists between each vehicle.This unknown integer is different for each satellite that both receiverstrack and must be estimated on a per-satellite basis. The integerambiguity problem is typically solved from GPS antenna to GPS antenna oneach vehicle. For a carrier phase of a given wavelength λ, the range toa GPS satellite i from a vehicle (designated as vehicle 1) is definedas:

λ({tilde over (φ)}₁ ^(i) +N ₁ ^(i))= ρ ₁ ^(i) +δP ₁ ^(E) +τ+b _(c) +v₁  (16)

where the carrier phase measurement at receiver 1 for satellite i isdefined as {tilde over (φ)}₁ ^(i), the integer number of wavelengthsbetween the receiver and satellite is N₁ ^(i), and an a priori estimateof the range to the satellite is ρ ₁ ^(i). This range is calculated fromthe known satellite locations and the a priori estimate of the GPSantenna either computed from the inertial state or from the GPS positionestimates or the pseudorange measurements. The carrier phase measurementis corrupted by errors in the estimate of the GPS antenna position δP₁^(E) that influences the error through a line of sight matrix C_(LOS)^(i). This matrix is defined as the vector line of sight from the GPSantenna on vehicle 1 to satellite i. In addition, a GPS receiver clockbias τ corrupts the measurements along with the receiver noise andcommon mode errors.

In the previous discussion, a single difference was defined as thedifference between two GPS measurements from two different vehicles. Nowa double difference is defined for the purpose of eliminating the commonmode errors and the clock bias. The double difference is defined as:

VΔ{tilde over (φ)}={tilde over (φ)} ₁ ^(i)−{tilde over (φ)}₁^(j)−({tilde over (φ)}₂ ^(i)−{tilde over (φ)}₂ ^(j))  (17)

In this case, satellite i and satellite j are different satellitescurrently in view by both receivers 1 and 2. The resulting measurementerror is defined as:

λ( VΔ{tilde over (φ)}+ VΔN)= VΔ ρ+(C _(LOS) ^(i) −C _(LOS) ^(j))ΔδP ^(E)VΔv _(φ)  (Eq. 18)

The definition of the double difference is applied to each term in Eq.(16). The common mode and receiver clock errors are now eliminated. Asimilar error model may be applied to create double differencedpseudorange GPS measurements.

VΔ{tilde over (ρ)}= VΔ ρ +(C _(LOS) ^(i) −C _(LOS) ^(j))ΔδP ^(E) + Vv_(ρ)  (Eq. 19)

Using the combination of these two measurements, it is possible toestimate the double differenced integer ambiguity and then utilize thedouble differenced carrier phase measurements in the GEKF. Severalmethods are available for performing this task. One way is to performcode minus carrier smoothing. Essentially, the difference between thepseudorange and carrier phase measurements is utilized to estimate theinteger bias through averaging over time:

$\begin{matrix}{{{\nabla\Delta}\; N} = {{\frac{1}{\lambda}{\nabla\Delta}\; \overset{\sim}{\rho}} - {{\nabla\Delta}\; \overset{\sim}{\varphi}}}} & \left( {{Eq}.\mspace{14mu} 20} \right)\end{matrix}$

Since VΔN is assumed constant, Eq. (20) is merely averaged over timeuntil the covariance is sufficiently small.

A more complex and precise method is to utilize the Wald Test for theinteger ambiguity problem. The Wald Test is a known statisticalhypothesis testing scheme. In this case, a set of possible integerambiguities is hypothesized and tested against the available GPSmeasurements. Using a combination of all of the available code andcarrier phase measurements available to both GPS receivers, a set ofresiduals are constructed and used to estimate the probability that aparticular hypothesis is correct. The residual formed as:

$\begin{matrix}{r_{k} = \begin{bmatrix}{{\lambda \left( {{{\nabla\Delta}\; \overset{\sim}{\varphi}} + {{\nabla\Delta}\; N_{k}}} \right)} - {{\nabla\Delta}\; \overset{\sim}{\rho}}} \\{E\; {\lambda \left( {{{\nabla\Delta}\overset{\sim}{\varphi}} + {{\nabla\Delta}\; N_{k}}} \right)}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 21} \right)\end{matrix}$

In this equation, the residual r_(k) is generated using a hypothesizedinteger ambiguity VΔN_(k) where k is the index number of the particularhypothesis. The measurements and residuals are now assumed toincorporate all of the satellites measurements available to both GPSreceivers. The number of hypotheses can be quite large since a range ofintegers must be hypothesized for each satellite. A special function Eis a matrix constructed as the left annihilator of the measurementsensitivity function. E is defined such that

E( VC _(LOS))=0  (Eq. 22)

where VC_(LOS) is the collection of the line of sight vectordifferences. The residual r_(k) has an assumed Gaussian probabilitydensity function, although the Wald Test allows for other densities tobe utilized. If the integer hypothesis k is correct, then the residualwill have a probability density function. Given a probability that thehypothesis is correct F_(k), and assumed density function for theresidual f_(k), the Wald Test calculates the probability that thehypothesis is correct using the residuals in recursive manner.

$\begin{matrix}{{F_{k}\left( {t + {\Delta \; t}} \right)} = \frac{{F_{k}(t)}{f_{k}\left( {t + {\Delta \; t}} \right)}}{\sum\limits_{k = 0}^{L}{{F_{k}(t)}{f_{k}\left( {t + {\Delta \; t}} \right)}}}} & \left( {{Eq}.\mspace{14mu} 23} \right)\end{matrix}$

If the probability of one hypothesis F_(k)(t+Δt) reaches one, then thathypothesis is declared the correct hypothesis. At this point the doubledifferenced carrier phase measurements can be utilized in the GEKF. Themeasurement model is updated utilizing the following form:

$\begin{matrix}{\left\lbrack \begin{matrix}{\overset{\sim}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\sim}{P}}_{GPS}^{E}} \\{\lambda \left( {{{\nabla\Delta}\; \overset{\sim}{\varphi}} + {{\nabla\Delta}\; N}} \right)}\end{matrix} \right\rbrack = {\left\lbrack \begin{matrix}{\overset{\_}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\_}{P}}_{GPS}^{E}} \\{{\nabla\Delta}\; \overset{\_}{\rho}}\end{matrix} \right\rbrack + {\left\lbrack \begin{matrix}C_{P\; 1} & 0 \\{C_{P\; 1} - C_{P\; 2}} & C_{P\; 2} \\0 & {\nabla\; C_{P\; 2}}\end{matrix} \right\rbrack\left\lbrack \begin{matrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{matrix} \right\rbrack} + \begin{bmatrix}b_{c} \\0 \\0\end{bmatrix} + \begin{bmatrix}v_{1} \\{\Delta \; v} \\{{\nabla\Delta}\; v_{\varphi}}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 24} \right)\end{matrix}$

This measurement function can now be used as part of the GEKF toestimate the relative and absolute state errors. It will be appreciatedthat this method applies for all of the different carrier phasewavelengths: for example, L1, L2, widelane, narrow lane, or any otherlinear combination. This above measurements may be modified to includedouble differenced code measurements as previously stated or to utilizeagain single differenced code or Doppler measurements provided that thestate is augmented to incorporate the relative state error.Sensor Fusion through the Global Extended Kalman Filter (GEKF)

As discussed previously, Eq (9) provides a convenient format for mixingin measurements from sensors such as GPS sensors or EO sensors. Forexample, This sensor fusion process may use the measurement function ofEq. (24) so as to fuse in differential GPS measurements. Eq (9) is nowrepeated for convenience.

$\begin{matrix}{\begin{bmatrix}{\delta \; {\overset{.}{x}}_{1}} \\{{\delta\Delta}\; \overset{.}{x}}\end{bmatrix} = {{\begin{bmatrix}A_{1} & 0 \\{A_{1} - A_{2}} & A_{2}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{{\delta\Delta}\; x}\end{bmatrix}} + {\begin{bmatrix}B_{1} & 0 \\B_{1} & {- B_{2}}\end{bmatrix}\begin{bmatrix}w_{1} \\{\Delta \; w}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 25} \right)\end{matrix}$

The dynamics of Equation (25) are driven by noise processes w₁ and Δw.The GEKF assumes that each of these is a Gaussian process with thefollowing statistics:

$\begin{matrix}{{{{E\left\lbrack \begin{bmatrix}w_{1} \\{\Delta \; w}\end{bmatrix} \right\rbrack} = 0};}{{E\left\lbrack {\begin{bmatrix}w_{1} \\{\Delta \; w}\end{bmatrix}\left\lbrack {w_{1}\mspace{20mu} \Delta \; w} \right\rbrack} \right\rbrack} = {\begin{bmatrix}W_{1} & W_{1} \\W_{1} & {W_{1} + W_{2}}\end{bmatrix} = W_{GEKF}}}} & \left( {{Eq}.\mspace{14mu} 26} \right)\end{matrix}$

where the covariance of the process noise for each inertial system isassumed independent of the other and have values denoted by W₁ and W₂.The term E[] represents the taking of the statistical expectationvalue. The process noise is determined by the inertial measurement unitquality. The initial statistics for the error states are likewise:

$\begin{matrix}{{{{E\left\lbrack \begin{bmatrix}{\delta \; x_{1}} \\{{\delta\Delta}\; x}\end{bmatrix} \right\rbrack} = 0};}{{E\left\lbrack {\begin{bmatrix}{\delta \; x_{1}} \\{{\delta\Delta}\; x}\end{bmatrix}\left\lbrack {\delta \; x_{1}\mspace{20mu} {\delta\Delta}\; x} \right\rbrack} \right\rbrack} = {\begin{bmatrix}P_{1} & P_{1} \\P_{1} & {P_{1} + P_{2}}\end{bmatrix} = P_{GEKF}}}} & \left( {{Eq}.\mspace{14mu} 27} \right)\end{matrix}$

The dynamic matrix for the inertial error dynamics are grouped togetherinto a common form as:

$\begin{matrix}{A_{GEKF} = \begin{bmatrix}A_{1} & 0 \\{A_{1} - A_{2}} & A_{2}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 28} \right)\end{matrix}$

The process noise sensitivity is defined as:

$\begin{matrix}{B_{EKF} = \begin{bmatrix}B_{1} & 0 \\B_{1} & {- B_{2}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 29} \right)\end{matrix}$

For a given time increment Δt, the state for each vehicle is integratedover time independently using Eq. (2) using a nonlinear integrationmethod of choice such as Euler integration or a Runge-Kutta method. Forexample, the tanker may be referred to as vehicle 1 and have its statedetermined through integration of the following:

$\begin{matrix}\begin{matrix}{{\overset{.}{x}}_{1} = \begin{bmatrix}{\overset{.}{P}}_{1}^{E} \\{\overset{.}{V}}_{1}^{E} \\{\overset{.}{Q}}_{B\; 1}^{E}\end{bmatrix}} \\{= \begin{bmatrix}V_{1}^{E} \\{{C_{B\; 1}^{E}a_{1}^{B\; 1}} - {\omega_{IE}^{E} \times \left( {\omega_{IE}^{E} \times P_{1}^{E}} \right)} - {2\omega_{IE}^{E} \times V_{1}^{E}} + g_{1}^{E}} \\{{\frac{1}{2}\left\lbrack {\Omega_{{EB}\; 1}^{B\; 1} \times} \right\rbrack}Q_{B\; 1}^{E}}\end{bmatrix}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 30} \right)\end{matrix}$

In this case, all terms denoted with a subscript “1” denote those valueor states experienced by the tanker. Similarly, the receiver aircraftmay be denoted as vehicle 2 and have corresponding subscripts:

$\begin{matrix}\begin{matrix}{{\overset{.}{x}}_{2} = \begin{bmatrix}{\overset{.}{P}}_{2}^{E} \\{\overset{.}{V}}_{2}^{E} \\{\overset{.}{Q}}_{B\; 2}^{E}\end{bmatrix}} \\{= \begin{bmatrix}V_{2}^{E} \\{{C_{B\; 2}^{E}a_{2}^{B\; 2}} - {\omega_{IE}^{E} \times \left( {\omega_{IE}^{E} \times P_{2}^{E}} \right)} - {2\omega_{IE}^{E} \times V_{2}^{E}} + g_{2}^{E}} \\{{\frac{1}{2}\left\lbrack {\Omega_{{EB}\; 2}^{B\; 2} \times} \right\rbrack}Q_{B\; 2}^{E}}\end{bmatrix}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 31} \right)\end{matrix}$

Each state is propagated forward at a user defined interval or at theinterval that the IMU is updated. At each new time interval, the valuesfor a₁ ^(B1) and ω_(IB1) ^(B1) are updated using the best estimates ormeasurements of the acceleration and angular velocity for the firstvehicle that are either estimated or measured from the IMU. Likewise,the values for a₂ ^(B2) and ω_(IB2) ^(B2) are updated using the bestestimates or measurements of the acceleration and angular velocity forthe second vehicle. However, the error covariance is propagated in timeusing the dynamic model described. The propagation is done as:

M _(GEKF)=Φ_(GEKF) P _(GEKF)(Φ_(GEKF))^(T)+Γ_(GEKF) W _(GEKF)Γ_(GEKF)^(T)  (Eq. 32)

where state transition matrix Φ_(GEKF) is defined as follows:

ΓΦ_(GEKF)=e^(A) ^(GEKF) ^(Δt)  (Eq. 33)

and where a discrete time process noise propagation matrix is definedas:

$\begin{matrix}{\Gamma_{GEKF} = {\int_{0}^{\Delta \; t}{^{A_{GEKF}\tau}{B_{EKF}(\tau)}{\tau}}}} & \left( {{Eq}.\mspace{14mu} 34} \right)\end{matrix}$

which may be approximated using appropriate means, if necessary.The measurement model described for the GPS measurements is repeated:

$\begin{matrix}{\left\lbrack \begin{matrix}{\overset{\sim}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\sim}{P}}_{GPS}^{E}} \\{\lambda\left( {{{\nabla\Delta}\; \overset{\sim}{\varphi}} + {{\nabla\Delta}\; N}} \right)}\end{matrix} \right\rbrack = {\left\lbrack \begin{matrix}{\overset{\_}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\_}{P}}_{GPS}^{E}} \\{{\nabla\Delta}\; \overset{\_}{\rho}}\end{matrix} \right\rbrack + \mspace{191mu} {\left\lbrack \begin{matrix}C_{P\; 1} & 0 \\{C_{P\; 1} - C_{P\; 2}} & C_{P\; 2} \\0 & {\nabla C_{P\; 2}}\end{matrix} \right\rbrack\left\lbrack \begin{matrix}{\delta \; x_{1}} \\{{\delta\Delta}\; x}\end{matrix} \right\rbrack} + \left\lbrack \begin{matrix}b_{c} \\0 \\0\end{matrix} \right\rbrack + \begin{bmatrix}v_{1} \\{\Delta \; v} \\{{\nabla\Delta}\; v_{\varphi}}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 35} \right)\end{matrix}$

The measurement noise is lumped together and assumed to be Gaussian withthe following statistics:

$\begin{matrix}{{{{E\left\lbrack \begin{bmatrix}{b_{c} + v_{1}} \\{\Delta \; v} \\{{\nabla\Delta}\; v_{\varphi}}\end{bmatrix} \right\rbrack} = 0};}\begin{matrix}{{E\left\lbrack {\begin{bmatrix}{b_{c} + v_{1}} \\{\Delta \; v} \\{{\nabla\Delta}\; v_{\varphi}}\end{bmatrix}\left\lbrack {b_{c} + {v_{1}\mspace{20mu} \Delta \; v\mspace{20mu} {\nabla\Delta} v_{\varphi}}} \right\rbrack} \right\rbrack} = {\quad{\begin{bmatrix}{V_{bc} + V_{1}} & V_{1} & 0 \\V_{1} & {V_{1} + V_{2}} & 0 \\0 & 0 & V_{\varphi}\end{bmatrix}\mspace{355mu} = V_{GEKF}}}} \\\mspace{149mu}\end{matrix}} & \left( {{Eq}.\mspace{14mu} 36} \right)\end{matrix}$

The measurement sensitivity matrix is lumped together into a commonmatrix defined as:

$\begin{matrix}{C_{GEKF} = \begin{bmatrix}C_{P\; 1} & 0 \\{C_{P\; 1} - C - {P\; 2}} & C_{P\; 2} \\0 & {\nabla C_{P\; 2}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 37} \right)\end{matrix}$

Whenever a GPS measurement is available, the inertial states arepropagated forward to the time of validity of the GPS measurements usingthe method outlined in Eq. (25) through Eq. (31). Then the GPSmeasurement is used to correct the state using the GEKF structure. Thecovariance is updated using the following covariance update formula:

P _(GEKF) =M _(GEKF) −M _(GEKF) C _(GEKF) ^(T)(C _(GEKF) M _(GEKF) C_(GEKF) ^(T) +V _(GEKF))⁻¹ C _(GEKF) M _(GEKF)  (Eq. 38)

A Kalman gain K is then calculated as:

K=P_(GEKF)C_(GEKF) ^(T)V_(GEKF) ⁻¹  (Eq. 39)

The state correction can be calculated as:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\hat{x}}_{1}} \\{{\delta\Delta}\; \hat{x}}\end{bmatrix} = {K\left( {\left\lbrack \begin{matrix}{\overset{\sim}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\sim}{P}}_{GPS}^{E}} \\{\lambda \left( {{{\nabla\Delta}\; \overset{\sim}{\varphi}} + {{\nabla\Delta}\; N}} \right)}\end{matrix} \right\rbrack - \left\lbrack \begin{matrix}{\overset{\_}{P}}_{{GPS}\; 1}^{E} \\{\Delta \; {\overset{\_}{P}}_{GPS}^{E}} \\{{\nabla\Delta}\; \overset{\_}{\rho}}\end{matrix} \right\rbrack} \right)}} & \left( {{Eq}.\mspace{14mu} 40} \right)\end{matrix}$

No a priori estimate of the error needs to be maintained since thecorrection is applied to the state estimate after each GPS measurement.The state is propagated forward in time using the nonlinear strap downequations of motion previously defined. The state corrections aredefined in terms of the corrections for the navigation state:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\hat{x}}_{1}} \\{{\delta\Delta}\; \hat{x}}\end{bmatrix} = \begin{bmatrix}{\delta \; P_{1}^{E}} \\{\delta \; V_{1}^{E}} \\{\delta \; q_{B\; 1}^{\overset{\_}{B}1}} \\{{\delta \; P_{1}^{E}} - {\delta \; P_{2}^{E}}} \\{{\delta \; V_{1}^{E}} - {\delta \; V_{2}^{E}}} \\{{\delta \; q_{B\; 1}^{\overset{\_}{B}1}} - {\delta \; q_{B\; 2}^{\overset{\_}{B}2}}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 41} \right)\end{matrix}$

The first vehicle state is corrected in the following manner to get theupdated estimate:

$\begin{matrix}{\begin{bmatrix}{\hat{P}}_{1}^{E} \\{\hat{V}}_{1}^{E} \\{\hat{Q}}_{B\; 1}^{E}\end{bmatrix} = \begin{bmatrix}{{\overset{\_}{P}}_{1}^{E} + {\delta \; P_{1}^{E}}} \\{{\hat{V}}_{1}^{E} + {\delta \; V_{1}^{E}}} \\{{\overset{\_}{Q}}_{\overset{\_}{B}1}^{E} \otimes {\hat{Q}}_{\hat{B}\; 1}^{\overset{\_}{B}\; 1}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 42} \right)\end{matrix}$

where a quaternion Q _(B1) ^(E) is the a priori attitude (before the GPSmeasurement is applied). The term (A

B) represents the standard rotation between two quaternions A and B. Inthis case, the a priori quaternion is corrected by applying thecorrective rotation {circumflex over (Q)}_({circumflex over (B)}1) ^(B1)which is defined using the quaternion correction δq_(B1) ^(B1) estimatedfrom the GEKF.

$\begin{matrix}{{\hat{Q}}_{\hat{B}1}^{\overset{\_}{B}1} = \begin{bmatrix}{1.0 - {{\delta \; q_{B\; 1}^{\overset{\_}{B}\; 1}}}} \\{\delta \; q_{B\; 1}^{\overset{\_}{B}\; 1}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 43} \right)\end{matrix}$

Similarly, the state of the second vehicle is corrected using:

$\begin{matrix}{\begin{bmatrix}{\hat{P}}_{2}^{E} \\{\hat{V}}_{2}^{E} \\{\hat{Q}}_{B\; 2}^{E}\end{bmatrix} = \begin{bmatrix}{{\overset{\_}{P}}_{2}^{E} + {\delta \; P_{2}^{E}}} \\{{\hat{V}}_{2}^{E} + {\delta \; V_{2}^{E}}} \\{{\overset{\_}{Q}}_{\overset{\_}{B}2}^{E} \otimes {\hat{Q}}_{\hat{B}\; 2}^{\overset{\_}{B}\; 2}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 44} \right)\end{matrix}$where:

$\begin{matrix}{{\hat{Q}}_{\hat{B}2}^{\overset{\_}{B}2} = \begin{bmatrix}{1.0 - {{\delta \; q_{B\; 2}^{\overset{\_}{B}\; 2}}}} \\{\delta \; q_{B\; 2}^{\overset{\_}{B}\; 2}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 45} \right)\end{matrix}$

Note that the process as shown assumed absolute position, singledifferenced position, and double differenced carrier phase measurements.If only some of these measurements are available, then the measurementsensitivity matrix C_(GEKF) and measurement noise V_(GEKF) are modifiedto include the proper number of measurements actually available and theprocessing proceeds as defined. Alternatively, the loosely coupledversions in which the GPS provides absolute position estimates ratherthan range measurements may be utilized by modifying the measurementfunction appropriately. Note also that the rate of propagation andcorrection do not have to be the same. The propagation process may berepeated numerous times between state updates copying M_(GEKF) intoP_(GEKF) and propagating the states at the same rate.

Sensor Fusion Using EO Measurements

EO measurements form a redundant set of measurements for the relativestate estimation error. The EO sensor typically operates on one vehicleand is utilized to image and provide measurements of the other vehicleor player. This EO system may be, for instance, utilized on the tankerto image a receiver aircraft and/or the boom. Typical EO measurementscome in one of two forms. In a first form, the EO system providesbearings measurements (elevation and azimuth) from the sensor to areference point on the imaged player. In a second form, the EO systemattempts to perform pattern matching between an image of the player anda set of recorded images. The first form is referred to as referencepoint mapping. The second form is referred to as silhouette matchingbecause the EO system attempts to match a possibly three-dimensional(3D) model to a two-dimensional image by first projecting the 3D modelonto the assumed image plane and forming a two-dimensional silhouette.

To perform sensor fusion between the EO sensor and the inertialnavigation state previously defined, the lever arm distance from theinertial state location on the vehicle to the reference point in viewmust be known. For example, with regard to reference points 1 through Nof FIG. 2 for the receiver aircraft, corresponding body frame lever armsbetween the receiver aircraft's IMU and the reference points should beknown. Using either EO technique or combinations thereof, themeasurements are related back to the location of the inertial state ofthe vehicle. Further, the EO sensor itself is often located at a fixedlever arm from the imaging vehicle inertial reference point (forexample, lever arm 135 for camera 130 of FIG. 1). The geometries of bothvehicles must be known or estimated as part of this procedure in orderto relate the image processing to the inertial navigation solutions. Themeasurement model of the vision system is defined relative to theinertial system on each system. The relative position between an EOsensor such as a camera and a given reference point is defined in termsof the relative position between each IMU in the ECEF coordinate frameas:

ΔP _(iC) ^(E) =P _(i) ^(E) −P _(C) ^(E) =P _(IMU2) ^(E) +C _(B) ₂ ^(E) L_(2-i) ^(B) ² −P _(IMU1) ^(E) −C _(B) ₁ ^(E) L _(1-C) ^(B) ¹   (Eq. 46)

where the position of the camera in the ECEF coordinate frame is P_(C)^(E), the position of the reference point i (such as one of referencepoints 1 through N of FIG. 2) on the target in the ECEF coordinate frameis P_(i) ^(E), and the relative position vector ΔP_(iC) ^(E) is thedifference between the two vectors. The relative position vector may bedefined in terms of the secondary vehicle IMU state P_(IMU2) ^(E) in theECEF coordinate frame plus the lever arm between the IMU and a referencelocation L_(1-i) ^(B) ² that must be rotated from the vehicle body frameinto the ECEF frame using a cosine rotation matrix C _(B) ₁ ^(E).Likewise, the primary vehicle's IMU position is defined as P_(IMU1) ^(E)and is located relative to the camera system through the lever armL_(1-C) ^(B) ¹ rotated from the primary vehicle body frame to the ECEFcoordinate frame through cosine rotation matrix C _(B) ₁ ^(E). Both C_(B) ₂ ^(E) and C _(B) ₁ ^(E) are calculated using the attitude of therespective vehicle. This relationship relates the camera system to theassociated inertial measurement unit through a reference frame that iscommon to the camera system, the IMU, and also the GPS system. Finally,a measurement model can be constructed which incorporates the GEKF stateestimates and enables the integration of the camera system within theexisting GEKF structure. A relative position vector in the camera frameis defined as:

$\begin{matrix}{{\Delta \; P_{iC}^{C}} = {\begin{bmatrix}{\Delta \; X_{iC}^{C}} \\{\Delta \; Y_{iC}^{C}} \\{\Delta \; Z_{iC}^{C}}\end{bmatrix} = \begin{bmatrix}{x_{1}^{C} - x_{2i}^{C}} \\{y_{1}^{C} - y_{2i}^{C}} \\{z_{1}^{C} - z_{2i}^{C}}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 47} \right)\end{matrix}$

Each of the terms is one component of the relative position vectorbetween the camera lens and the reference point on the receiver relatedin the camera reference frame.

EO Bearing Measurements

The measurements from an EO system may provide complete positionestimates to active beacons or a camera system may simply identifypoints and provide bearings measurements from the camera to thereference point. Bearings measurements may be made up of two anglesdefined as:

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{\alpha}}_{iC} \\{\overset{\sim}{\beta}}_{iC}\end{bmatrix} = {\begin{bmatrix}{\tan^{- 1}\left( \frac{\Delta \; Z_{iC}^{C}}{\Delta \; X_{i\; C}^{C}} \right)} \\{\tan^{- 1}\left( \frac{\Delta \; Y_{i\; C}^{C}}{\Delta \; X_{i\; C}^{C}} \right)}\end{bmatrix} + \begin{bmatrix}\upsilon_{\alpha} \\\upsilon_{\beta}\end{bmatrix}}} & \left( {{{Eq}.\mspace{14mu} 47}a} \right)\end{matrix}$

where C_(E) ^(C) α_(iC) represents the azimuth angle of the referencepoint i relative to the camera C in the camera frame, and β_(iC) ^(C)represents the elevation angle of the target. The additive noise termsare zero mean, Gaussian with noise variance associated with the errormodels described previously including pixel noise and blurring effects.Each of the terms is one component of the relative position vectorbetween the camera lens and the reference point on the receiver relatedin the camera reference frame.

A Modified Gain Extended Kalman Filter (MGEKF) may be used to translatethe elevation and azimuth angles to a Cartesian position error in thecamera frame. Advantageously, this filter will have zero mean steadystate error. The error function for the measurements (in the EO sensorframe) is now defined. First, the residual process is defined as thedifference between the measured and a priori estimates of the angles as:

$\begin{matrix}\begin{matrix}{r_{i} = \begin{bmatrix}{\alpha_{i} - {\overset{\_}{\alpha}}_{i}} \\{\beta_{i} - {\overset{\_}{\beta}}_{i}}\end{bmatrix}} \\{= {\begin{bmatrix}{{\tan^{- 1}\left( \frac{y_{1}^{C} - y_{2\; i}^{C}}{x_{1}^{C} - x_{2\; i}^{C}} \right)} - {\tan^{- 1}\left( \frac{{\overset{\_}{y}}_{1}^{C} - {\overset{\_}{y}}_{2\; i}^{C}}{{\overset{\_}{x}}_{1}^{C} - {\overset{\_}{x}}_{2\; i}^{C}} \right)}} \\{{\tan^{- 1}\left( \frac{z_{1}^{C} - z_{2\; i}^{C}}{x_{1}^{C} - x_{2\; i}^{C}} \right)} - {\tan^{- 1}\left( \frac{{\overset{\_}{z}}_{1}^{C} - {\overset{\_}{z}}_{2\; i}^{C}}{{\overset{\_}{x}}_{1}^{C} - {\overset{\_}{x}}_{2\; i}^{C}} \right)}}\end{bmatrix}{\bullet \begin{bmatrix}{\tan^{- 1}(\Theta)} \\{\tan^{- 1}(\Psi)}\end{bmatrix}}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 48} \right)\end{matrix}$

where the angles Θ and ψ are defined using the following relationship:

$\begin{matrix}{{{\tan^{- 1}(a)} - {\tan^{- 1}(b)}} = {\tan^{- 1}\left( \frac{a - b}{1 + {ab}} \right)}} & \left( {{Eq}.\mspace{14mu} 49} \right)\end{matrix}$

Therefore the residual process may be re-written in the following way:

$\begin{matrix}{r_{i} = {\begin{bmatrix}{{\overset{\sim}{\alpha}}_{i} - {\overset{\sim}{\alpha}}_{i}} \\{{\overset{\sim}{\beta}}_{i} - {\overset{\_}{\beta}}_{i}}\end{bmatrix} = {\begin{bmatrix}{{- D_{1}}{\tan^{- 1}(\Theta)}\text{/}\Theta} & 0 \\0 & {{- D_{2}}{\tan^{- 1}(\Psi)}\text{/}\Psi}\end{bmatrix}{\quad{\begin{bmatrix}{\sin \left( {\overset{\sim}{\alpha}}_{i} \right)} & 0 & {- {\cos \left( {\overset{\sim}{\alpha}}_{i} \right)}} \\{\sin \left( {\overset{\sim}{\beta}}_{i} \right)} & {- {\cos \left( {\overset{\sim}{\beta}}_{i} \right)}} & 0\end{bmatrix}\begin{bmatrix}{x_{1}^{C} - x_{2\; i}^{C}} \\{y_{1}^{C} - y_{2\; i}^{C}} \\{z_{1}^{C} - z_{2\; i}^{C}}\end{bmatrix}}}}}} & \left( {{Eq}.\mspace{14mu} 50} \right)\end{matrix}$where:

$\begin{matrix}\begin{matrix}{D_{1} = {1/\left\lbrack {{{\cos \left( {\overset{\sim}{\alpha}}_{i} \right)}\left( {{\overset{\_}{x}}_{1}^{C} - {\overset{\_}{x}}_{2i}^{C}} \right)} + {{\sin \left( {\overset{\sim}{\alpha}}_{i} \right)}\left( {{\overset{\_}{z}}_{1}^{C} - {\overset{\_}{z}}_{2i}^{C}} \right)}} \right\rbrack}} \\{D_{2} = {1/\left\lbrack {{{\cos \left( {\overset{\sim}{\beta}}_{i} \right)}\left( {{\overset{\_}{x}}_{1}^{C} - {\overset{\_}{x}}_{2\; i}^{C}} \right)} + {{\sin \left( {\overset{\sim}{\beta}}_{i} \right)}\left( {{\overset{\_}{y}}_{1}^{C} - {\overset{\_}{y}}_{2i}^{C}} \right)}} \right\rbrack}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 51} \right)\end{matrix}$

The modified gain for the measurement function may be defined as:

$\begin{matrix}{H_{MGEKFi} = \begin{bmatrix}{{- D_{1}}{{\tan^{- 1}(\Theta)}/\Theta}} & 0 \\0 & {{- D_{2}}{{\tan^{- 1}(\Psi)}/\Psi}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 52} \right)\end{matrix}$

The line of sight matrix formed directly from the measurements

$\begin{matrix}{H_{Los\_ i} = \begin{bmatrix}{\sin \left( {\overset{\sim}{\alpha}}_{i} \right)} & 0 & {- {\cos \left( {\overset{\sim}{\alpha}}_{i} \right)}} \\{\sin \left( {\overset{\sim}{\beta}}_{i} \right)} & {- {\cos \left( {\overset{\sim}{\beta}}_{i} \right)}} & 0\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 53} \right)\end{matrix}$

The error in the bearings measurements in the camera frame may now bewritten as:

$\begin{matrix}{\begin{bmatrix}{{\overset{\sim}{\alpha}}_{i} - {\overset{\_}{\alpha}}_{i}} \\{{\overset{\sim}{\beta}}_{i} - {\overset{\_}{\beta}}_{i}}\end{bmatrix} = {H_{MGEKFi}{H_{Los\_ i}\begin{bmatrix}{x_{1}^{C} - x_{2i}^{C}} \\{y_{1}^{C} - y_{2\; i}^{C}} \\{z_{1}^{C} - z_{2i}^{C}}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 54} \right)\end{matrix}$

In this fashion, the measurement model becomes:

$\begin{matrix}{{\begin{bmatrix}{\overset{\sim}{\alpha}}_{iC} \\{\overset{\sim}{\beta}}_{iC}\end{bmatrix} - \begin{bmatrix}{\overset{\_}{\alpha}}_{iC} \\{\overset{\_}{\beta}}_{iC}\end{bmatrix}} = {{H_{MGEKFi}H_{Los\_ i}{H_{IMUtoC}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{bmatrix}}} + \begin{bmatrix}\upsilon_{\alpha} \\\upsilon_{\beta}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 55} \right)\end{matrix}$

In the above discussion, the a priori values for the angles arecalculated using the a priori state estimates from the GEKF. The matrixH_(Los) _(—) _(i) has the line-of-sight vector for the ith referencepoint and the matrix H_(MGEKF) is generated as part of the MGEKF and isdesigned such that the residual calculated is zero mean in the ECEFCartesian coordinate frame. Finally, the matrix H_(IMUtoC) is a matrixwhich translates the inertial error states of the Global Extended KalmanFilter to the position error in the camera frame and is similar to theGPS measurement matrix but utilizing the camera and reference framelever arms instead of the lever arms to the GPS antenna. Thismeasurement model therefore relates the camera system elevation andazimuth angles of the reference point to the error in the inertialsystem on the primary vehicle as the camera as well as the relativeinertial error between the primary vehicle and the secondary vehicle.

The matrix H_(IMUtoC) is defined using a similar methodology as thetranslation from the IMU to the GPS antenna location. However, theresults are not as trivial. To estimate the error between each inertialframe in the EO sensor reference frame defined in Eq. (47), theperturbation must be taken around the relative state vector:

ΔP _(iC) ^(C) =C _(B) ₁ ^(C) C _(E) ^(B) ¹ (P _(i) ^(E) −P _(C) ^(E))=C_(B) ₁ ^(C) C _(E) ^(B) ¹ (P ₂ ^(E) +C _(B) ₂ ^(E) L _(2-i) ^(B) ² −P ₁^(E) −C _(B) ₁ ^(E) L _(1-C) ^(B) ¹ )  (Eq. 56)

In this case, the lever arms L_(2-i) ^(B) ² and L_(1-C) ^(B) ¹ areassumed known as is the orientation of the camera relative to theinertial system on vehicle 1, C_(B1) ^(E).The following perturbations are defined as:

P ₁ ^(E) = P ₁ ^(E) +δP ₁  (Eq. 57)

P ₂ ^(E) = P ₂ ^(E) +δP ₂  (Eq. 58)

C _(B1) ^(E) =C _(B1) ^(E)(I+2[δq _(B1) ^(B1)×])  (Eq. 59)

C _(B2) ^(E) =C _(B2) ^(E)(I+2=[δq _(B2) ^(B2)×])  (Eq. 60)

Then the perturbations are substituted back into Eq. (56) to form:

$\begin{matrix}{{\Delta \; P_{iC}^{C}} = {{{C_{B\; 1}^{C}\left( {I - {2\left\lbrack {\delta \; q_{1} \times} \right\rbrack}} \right)}{C_{E}^{{\overset{\_}{B}}_{1}}\left( {{\overset{\_}{P}}_{2}^{E} + {\delta \; P_{2}} - {\overset{\_}{P}}_{1}^{E} - {\delta \; P_{1}}} \right)}} - {C_{B_{1}}^{V}L_{1 - C}^{B_{1}}} + {{C_{B_{1}}^{C}\left( {I - {2\left\lbrack {\delta \; q_{1} \times} \right\rbrack}} \right)}C_{E}^{{\overset{\_}{B}}_{1}}{C_{{\overset{\_}{B}}_{2}}^{E}\left( {I + {2\left\lbrack {\delta \; q_{2} \times} \right\rbrack}} \right)}L_{2 - i}^{B\; 2}} - \left( {{C_{B_{1}}^{V}{C_{E}^{\overset{\_}{B}1}\left( {{\overset{\_}{P}}_{2}^{E} - {\overset{\_}{P}}_{1}^{E}} \right)}} - {C_{B\; 1}^{C}L_{1 - C}^{B\; 1}} + {C_{B\; 1}^{C}C_{E}^{{\overset{\_}{B}}_{1}}C_{\overset{\_}{B}2}^{E}L_{2 - i}^{B\; 2}}} \right)}} & \left( {{Eq}.\mspace{14mu} 61} \right)\end{matrix}$

Regrouping the terms of Eq. (61) into the GEKF state space and excludinghigher order terms of perturbations produces:

$\begin{matrix}{{\Delta \; P_{iC}^{C}} \approx {{\Delta \; {\overset{\_}{P}}_{iC}^{C}} + {{C_{B\; 1}^{C}\begin{bmatrix}0 & 0 & H_{Q\; 1} & C_{E}^{\overset{\_}{B}1} & 0 & \left( {H_{Q\; 1} + H_{Q\; 2}} \right)\end{bmatrix}}\begin{bmatrix}{\delta \; P_{1}} \\{\delta \; V_{1}} \\{\delta \; q_{B\; 1}^{\overset{\_}{B}1}} \\{{\Delta\delta}\; P} \\{{\Delta\delta}\; V} \\{{\Delta\delta}\; q}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 62} \right)\end{matrix}$

The a priori estimate Δ P _(iC) ^(C) is defined as:

Δ P _(iC) ^(C) =C _(B) ₁ ^(C) C _(E) ^(B) ¹ ( P ₂ ^(E) +C _(B) ₂ ^(E) L_(2-i) ^(B) ² − P ₁ ^(E) −C _(B) ₁ ^(E) L _(1-C) ^(B) ¹   (Eq. 63)

The term H_(Q1) is defined as:

H _(Q1)=2([C _(E) ^(B) ¹ P ₁ ^(E) − P ₂ ^(E))×]+[(C _(E) ^(B) ¹ C _(B) ₂^(E) L _(2-i) ^(B2))×]  (Eq. 64)

The term H_(Q2) is defined as:

H _(Q2)=2C _(E) ^(B) ¹ C _(B) ₂ ^(E)[(L _(2-i) ^(B) ² )×]  (Eq. 65)

A notable portion of Eq. (62) is that, since the camera measurements arerelated through the inertial frame, the term H_(Q1) actually provideslimited observability of the error in the absolute state of vehicle 1.In other words, the absolute state of vehicle 1 relative to the Earth isobservable through the camera measurements. This additionalobservability is a direct result of the fact that the camerameasurements which provide relative state information are relatedthrough a common inertial reference frame.

A matrix H_(IMU2C) may be defined as:

H _(IMU2) C=C _(B1) ^(C)[00H _(Q1) C _(C) ^(B10)(H _(Q1) +H_(Q2))]  (Eq. 66)

The measurement function in Eq. (55) is sufficient to provide relativenavigation solutions to the GEKF previously defined given sufficientnumbers of reference points. Each reference point may take the form ofan active beacon on the viewed vehicle or a small, recognizable portionof the vehicle with known location relative to the inertial system.

EO Silhouette Matching Measurements

A single silhouette measurement produces a complete state of thefollowing form:

$\begin{matrix}{{\overset{\sim}{y}}_{i} = \begin{bmatrix}{\overset{\sim}{\rho}}_{i} \\{\overset{\sim}{\alpha}}_{i} \\{\overset{\sim}{\beta}}_{i} \\Q_{B_{2}}^{C}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 67} \right)\end{matrix}$

where {tilde over (α)}_(i) and {tilde over (β)}_(i) have the same angledefinition as before. The new variable {tilde over (ρ)}_(i) representsthe scalar range from the vision system to the target along the vectorline defined by {tilde over (α)}_(i) and {tilde over (β)}_(i). Thequaternion is the estimated attitude of the target relative to thecamera system.The total measurement equation is:

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{\rho}}_{i} \\{\overset{\sim}{\alpha}}_{i} \\{\overset{\sim}{\beta}}_{i}\end{bmatrix} = {\begin{bmatrix}{\overset{\_}{\rho}}_{i} \\{\overset{\_}{\alpha}}_{i} \\{\overset{\_}{\beta}}_{i}\end{bmatrix} + {\begin{bmatrix}H_{\rho \; i} \\H_{C\; \alpha} \\H_{C\; \beta}\end{bmatrix}{H_{{IMU}\; 2C}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{bmatrix}}} + \begin{bmatrix}v_{\rho} \\v_{\alpha} \\v_{\beta}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 68} \right)\end{matrix}$

The a priori relative range measurement is calculated as the norm of therelative state vector or:

ρ=∥ΔP_(iC) ^(C)∥  (Eq. 69)

Here, the range measurement matrix H_(ρi) is defined as:

$\begin{matrix}{H_{\rho \; i} = \begin{bmatrix}\frac{\Delta \; {\overset{\_}{X}}_{i\; C}^{C}}{{\Delta \; {\overset{\_}{P}}_{i}^{C}}} & \frac{\Delta {\overset{\_}{Y}}_{iC}^{C}}{{\Delta \; {\overset{\_}{P}}_{i}^{C}}} & \frac{\Delta \; {\overset{\_}{Z}}_{i\; C}^{C}}{{\Delta \; {\overset{\_}{P}}_{i}^{C}}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 70} \right)\end{matrix}$

The combined bearings measurement matrices are equivalent to theprevious forms as:

$\begin{matrix}{\begin{bmatrix}H_{C\; \alpha} \\H_{C\; \beta}\end{bmatrix} = {H_{MGEKF}H_{LOS\_ i}}} & \left( {{Eq}.\mspace{14mu} 71} \right)\end{matrix}$

The matrix H_(IMU2C) is as defined in Eq. (66).

The error from the attitude is treated separately since the residual isa set of relative attitude angles and is not related to the relativedistance as the range, azimuth, and elevation are related to range. Theattitude determined from an image defines the rotation from the vehiclebody frame of the imaged vehicle to the camera frame. The measurementfunction is nonlinear:

{tilde over (C)} _(B) ₂ ^(C) =C _(B) ₂ ^(C)(I+2[δq ₂×])(I+[v_(q)×])  (Eq. 72)

In this case, the term {tilde over (C)}_(B) ₂ ^(C) is the rotationmatrix determined from the estimated relative yaw, pitch and roll of theimaged vehicle in the camera produced image. The a priori estimate ofthe rotation is C _(B) ₂ ^(C). Two errors are associated with therotation. The first is due to the error in the estimate of the imagedvehicle. This is comprised of the usual attitude quaternion perturbationδq₂. An additional, nonlinear rotation error is due to errors in theimage processing v_(q). The measurement equation may be re-written interms of a linearized residual in which higher order terms of therotation error and noise are neglected:

$\begin{matrix}{r_{q} = {\begin{bmatrix}\left( {{\overset{\sim}{C}}_{{\overset{\sim}{B}}_{2}}^{C}C_{C}^{{\overset{\_}{B}}_{2}}} \right)_{32} \\\left( {{\overset{\sim}{C}}_{{\overset{\sim}{B}}_{2}}^{C}C_{C}^{{\overset{\_}{B}}_{2}}} \right)_{13} \\\left( {{\overset{\sim}{C}}_{{\overset{\sim}{B}}_{2}}^{C}C_{C}^{{\overset{\_}{B}}_{2}}} \right)_{21}\end{bmatrix} = {{H_{q}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{bmatrix}} + \begin{bmatrix}v_{\Phi} \\v_{\Theta} \\v_{\Psi}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 73} \right)\end{matrix}$

where the form ({tilde over (C)} _(B) ₂ ^(C)C_(C) ^(B) ² )_(ij) is theith row and jth column of the 3×3 matrix {tilde over (C)} _(B) ₂^(C)C_(C) ^(B) ² .The quaternion measurement matrix H_(q) is simply defined as:

H _(q)=[002I00−2I]  (Eq. 74)

The measurement noise v_(q) is decomposed into the axial components as:

$\begin{matrix}{v_{q} = \begin{bmatrix}v_{\Phi} \\v_{\Theta} \\v_{\Psi}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 75} \right)\end{matrix}$

Therefore, the residual is formed using Eq. (73) and the associatederror models. The error models are then utilized to form and correct theGEKF already defined.

Note that the above algorithm assumes that vehicle 1 images vehicle 2using either reference points or silhouette matching. However, it istrivial to expand the results to include additional measurements ofvehicle 2 imaging vehicle 1. For example, the receiver aircraft mayinclude a camera (not illustrated in FIG. 2). Further, multiplesilhouettes may be generated for different parts of the vehicle orplayer in view. Using either bearings only with multiple referencepoints or a silhouette matching scheme or combinations thereof, it ispossible to use the measurement models defined in Eq. (68) and/or Eq.(55) to correct the inertial errors with error modeling defined in Eq.(15). Finally, the results may be simplified to the case where only arange measurement is available.

Processing Additional Sensor Measurements Through the GEKF

The Global Extended Kalman Filter (GEKF) has been defined for processingthe GPS, EO, and INS measurements. However, the GEKF may be extended toinclude additional measurements such as relative range, bearingsmeasurements, and relative attitude that may be provided from anElectro-Optic sensor or vision processing. The measurement functionsderived can be used to correct the GEKF presented previously. Thecomplete measurement function becomes:

$\begin{matrix}{\begin{bmatrix}{{\overset{\sim}{\rho}}_{i} - {\overset{\_}{\rho}}_{i}} \\{{\overset{\sim}{\alpha}}_{i} - {\overset{\_}{\alpha}}_{i}} \\{{\overset{\sim}{\beta}}_{i} - {\overset{\_}{\beta}}_{i}} \\r_{q}\end{bmatrix} = {{\begin{bmatrix}{H_{\rho \; i}H_{{IMU}\; 2\; C}} \\{H_{C\; \alpha}H_{{IMU}\; 2\; C}} \\{H_{C\; \beta}H_{{IMU}\; 2\; C}} \\H_{q}\end{bmatrix}\begin{bmatrix}{\delta \; x_{1}} \\{\delta \; \Delta \; x}\end{bmatrix}} + \begin{bmatrix}v_{\rho} \\v_{\alpha} \\v_{\beta} \\v_{q}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 76} \right)\end{matrix}$

The measurement noise is assumed to be Gaussian with the followingstatistics.

$\begin{matrix}{{{E\left\lbrack \begin{bmatrix}v_{\rho} \\v_{\alpha} \\v_{\beta} \\v_{q}\end{bmatrix} \right\rbrack} = 0};{{E\left\lbrack {\begin{bmatrix}v_{\rho} \\v_{\alpha} \\v_{\beta} \\v_{q}\end{bmatrix}\begin{bmatrix}v_{\rho} & v_{\alpha} & v_{\beta} & v_{q}\end{bmatrix}} \right\rbrack} = V_{EO}}} & \left( {{Eq}.\mspace{14mu} 77} \right)\end{matrix}$

Note that V_(EO) is a covariance which contains any and all of thecorrelations between the noise processes, as determined by the methodutilized to generate the range, angles, and attitude measurements. It isup to the designer to select appropriate values for V_(EO). Likewise,the measurement sensitivity is grouped together into a single matrix forconvenience of notation:

$\begin{matrix}{H_{EO} = \begin{bmatrix}{H_{\rho \; i}H_{{IMU}\; 2\; C}} \\{H_{C\; \alpha}H_{{IMU}\; 2\; C}} \\{H_{C\; \beta}H_{{IMU}\; 2\; C}} \\H_{q}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 78} \right)\end{matrix}$

Therefore, the GEKF can be modified to include the vision systemmeasurements. The inertial navigation state is propagated forward intime by an amount Δt using the process and methods described in Eq. (25)through Eq. (34). At that time, if GPS measurements are available, thenavigation state may be updated and corrected using the GPS measurementfunction and EKF corrections outlined in Eq. (35) through Eq. (45).After the state and covariance are updated, the system may check for EOmeasurements. If GPS measurements were available and applied, then thestate is the most up to date state using the most recent GPS and IMUmeasurements. The covariance P_(GEKF) is copied into M_(GEKF) beforeprocessing the EO measurements.

If electro-optic (EO) sensor measurements are available, then thefollowing method may be used to correct the navigation state through aGEKF process with or without GPS measurements. First, the covariance isupdated using the following method:

P _(GEKF) =M _(GEKF) −M _(GEKF) H _(EO) ^(T)(H _(EO) M _(GEKF) H _(EO)^(T) +V _(EO))⁻¹ H _(EO) M _(GEKF)  (Eq. 79)

The Kalman Gain is then calculated as:

K=P_(GEKF)H_(EO) ^(T)V_(EO) ⁻¹  (Eq. 80)

The state correction can be calculated as:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\hat{x}}_{1}} \\{\delta \; \Delta \; \hat{x}}\end{bmatrix} = {K\left( \begin{bmatrix}{{\overset{\sim}{\rho}}_{i} - {\overset{\_}{\rho}}_{i}} \\{{\overset{\sim}{\alpha}}_{i} - {\overset{\_}{\alpha}}_{i}} \\{{\overset{\sim}{\beta}}_{i} - {\overset{\_}{\beta}}_{i}} \\r_{q}\end{bmatrix} \right)}} & \left( {{Eq}.\mspace{14mu} 81} \right)\end{matrix}$

No a priori estimate of the error needs to be maintained since thecorrection is applied to the state estimate. The state corrections aredefined in terms of the corrections for the navigation state:

$\begin{matrix}{\begin{bmatrix}{\delta \; {\hat{x}}_{1}} \\{\delta \; \Delta \; \hat{x}}\end{bmatrix} = \begin{bmatrix}{\delta \; P_{1}^{E}} \\{\delta \; V_{1}^{E}} \\{\delta \; q_{B\; 1}^{\overset{\_}{B}1}} \\{{\delta \; P_{1}^{E}} - {\delta \; P_{2}^{E}}} \\{{\delta \; V_{1}^{E}} - {\delta \; V_{2}^{E}}} \\{{\delta \; q_{B\; 1}^{\overset{\_}{B}1}} - {\delta \; q_{B\; 2}^{\overset{\_}{B}2}}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 82} \right)\end{matrix}$

The correction process is applied as discussed for Eq. (42) through Eq.(45). Note that this correction may take place even if GPS measurementsare not available. In this case, the INS states and covariances arepropagated forward in time to the current reference time of the EOmeasurements utilizing the INS data. Then the EO measurement correctionprocess is applied. Similarly, this correction process may take placebefore the GPS measurements are utilized to correct the state providedthat the navigation state and covariance was corrected with the EOmeasurements before processing the GPS. Finally, a third option is thatthe measurement functions and noise covariances from both the EO and GPSmeasurements available are combined into one single update function forthe GEKF. This last segment only applies if the GPS and EO measurementshave the same reference time for the measurements. Otherwise, the statemust be propagated forward in time to the next measurement whether it isthe EO or GPS measurements.

Processing Radio Ranging Measurements Through the GEKF

Measurements from radio ranging devices or radio bearings devices may beused in the GEKF. In this case, the radio provides a measurement of therelative range to the other vehicle radio. Multiple antennae may provideelevation and/or azimuth angles from the first vehicle to the relativevehicle. Such applications include data links which provide range usingtime delay of arrival schemes or Auto Collision Avoidance Systems whichattempt which provide time delay of arrival of message or angles to thesource of a transmission. The measurement model is simply modified toinclude only those measurements present. Instead of an EO sensor, themeasurements are taken at the radio antenna which must be referencedrelative to the IMU in the same way that the EO sensor lever arm wasdefined. In other words L_(1-C) ^(B) ¹ becomes the vector arm from theIMU to the radio antenna rather than the camera. Further, if elevationand azimuth are defined relative to an antenna, the relative attitudebetween the antenna and the inertial must be defined. In other words,C_(B) ₁ ^(C) is replaced with the rotation matrix from the IMU bodyframe to the antenna frame which should be constant for a rigid body.Likewise, the antenna location on the second vehicle must be surveyedrelative to the second vehicle IMU. In other words, L_(2-i) ^(B) ² isreplaced with the lever arm from the IMU on the second vehicle to theradio antenna on the second vehicle. Using these three replacements, themeasurement models remain valid and may be utilized to incorporate radioranging from antenna to antenna on different vehicles.

Fault Detection

As discussed with regard to FIGS. 1 and 2, a statistically optimalautomatic control methodology has been disclosed for the control of anaerial refueling boom. But it may be the case that sensor measurementsare defective. For example, GPS signal errors include but are notlimited to multi-path, satellite transmission errors, or simply lowsignal-to-noise due to partial blockage by the tanker. A receiverfailure can be caused by a variety of factors, from signal blockage bythe tanker to combat damage. Only the detection of more subtle GPSsignal errors are discussed below since the detection of a failedreceiver is trivial with such methods as utilizing watchdog timers onthe output of the receiver. Turning now to FIG. 3, a fault detection andreconfiguration process is outlined. A bank of fault detection filtersand residual generator 300 processes measurements from, for example, anIMU, a GPS receiver, and an EO system. Residuals from generator 300 areprocessed in a residual processor 305 and a fault reconstruction process310 to provide fault probabilities and fault magnitudes, respectively. Afault tolerant estimator 315 processes the fault probabilities and faultmagnitudes to provide fault-free inertial navigation state estimates.The fault estimation and reconfiguration process of FIG. 3 will be morefully explained below.

Absolute GPS Fault Detection

GPS signal errors introduce an effective, unknown bias on one of the GPSsatellite range and/or carrier phase measurements relative to the otherGPS measurements. A least squares approach may be used to detect andisolate failures in a single GPS satellite measurement and isolate itfrom the remaining GPS measurement sets. For example, a MultipleHypothesis Shiryayev Sequential Probability Ratio Test (MHSSPRT) may beused for detection and isolation of these errors (this test is describedfurther below).

A least squares GPS navigation solution is well known. The pseudo rangemeasurements from four or more visible GPS satellites are combined toform an estimate of the GPS receiver position and clock bias relative tothe GPS constellation. Given n satellites, the navigation solution canbe computed with (n−1) satellites, cycling through the satellites tocreate (n−1) solutions. Comparing the residual of these solutions overtime using the SSRPT creates a probability distribution that indicatesthe likelihood of a satellite failure. When the probability that asatellite is failing reaches 99.9% or a required alert probability, analarm is triggered and the satellite is no longer used for computationof the navigation state.

The range from a GPS receiver to a satellite i is denoted by the symbolρ_(i) and defined as:

ρ_(i)=√{square root over ((X _(i) −x)²+(Y _(i) −y)²+(Z _(i)−z)²)}{square root over ((X _(i) −x)²+(Y _(i) −y)²+(Z _(i) −z)²)}{squareroot over ((X _(i) −x)²+(Y _(i) −y)²+(Z _(i) −z)²)}+cτ  (Eq. 83)

The range measurement is a function of a satellite position vector P_(i)^(E), a receiver position vector p ^(E), and a receiver clock bias τ.The variable c represents the speed of light and converts a clock biasin seconds to a range. The satellite position vector in the ECEFcoordinate frame is defined as:

$\begin{matrix}{P_{i}^{E} = \begin{bmatrix}X_{i} \\Y_{i} \\Z_{i}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 84} \right)\end{matrix}$

The receiver position in the Earth Centered Earth Fixed coordinate frameis defined as:

$\begin{matrix}{P^{E} = \begin{bmatrix}x \\y \\z\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 85} \right)\end{matrix}$

Given an a priori estimate of the receiver position and clock bias, theestimated range to the satellite is given by:

ρ=√{square root over ((X _(i) − x )²+(Y _(i) − y )²+(Z _(i) − z )²)}+c τ  (Eq. 86)

In this case the terms denoted by ( ) represent a priori estimates. Theestimated range is in error and a linear perturbation defines the errorbetween the estimated range and the true range.

ρ_(i)= ρ _(i) +H _(i) δx  (Eq. 87)

where the variable δx is the perturbed error in the state and consistsof a perturbation in the state estimate which is to be estimated. Theperturbation is defined as truth minus the estimated, or:

$\begin{matrix}{{\delta \; x} = {\begin{bmatrix}x \\y \\z \\{c\; \tau}\end{bmatrix} - \begin{bmatrix}\overset{\_}{x} \\\overset{\_}{y} \\\overset{\_}{z} \\{c\; \overset{\_}{\tau}}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 88} \right)\end{matrix}$

A line of sight matrix H_(i) is defined as:

$\begin{matrix}{H_{i} = \begin{bmatrix}\begin{matrix}\frac{\left( {X_{i} - \overset{\_}{x}} \right)}{{P_{i}^{E} - \overset{\_}{P}}} & \frac{\left( {Y_{i} - \overset{\_}{y}} \right)}{{P_{i}^{E} - \overset{\_}{P}}} & \frac{\left( {Z_{i} - \overset{\_}{z}} \right)}{{P_{i}^{E} - \overset{\_}{P}}}\end{matrix} & 1\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 89} \right)\end{matrix}$

where the term ∥∥ denotes the 2-norm (sum of the squares) of thevector. From these definitions, a set of GPS measurements may be definedas:

{tilde over (ρ)}_(i)= ρ _(i) +H _(i) δx+μ _(i) +v _(i) +b _(i)  (Eq. 90)

The GPS measurement of range {tilde over (p)} is corrupted by randomnoise v_(i) which is zero mean and has variance V_(i), as well as commonmode errors b_(i) which include ionosphere and troposphere errors. Thecommon mode errors are typically not zero mean, but are nearly the samefor two GPS receivers within close proximity. In addition, an unknownfault μ_(i) may exist in the measurement such as multipath. A goal is toestimate the state error δx using a set of measurements {tilde over(ρ)}, one of which may include a fault μ_(i) in the ith measurement, andthen detect and isolate the satellite with the fault. In the followingdiscussion, only one satellite fault is assumed.

The tanker and receiver aircraft may each receive a set of GPSmeasurements of the form in Eq. (90).

{tilde over (ρ)}_(tank)= ρ _(tank) +H _(tank) δx _(tank) +v_(tank)  (Eq. 91)

{tilde over (ρ)}_(rec)= ρ _(rec) +H _(rec) δx _(rec) +V _(rec)  (Eq. 92)

The common mode errors and fault modes are neglected for the moment. Aweighted least squares solution is provided for the receiver and tankeraircraft below. The solutions presented represent the best estimate ofthe error for each vehicle using only the data available from each GPSreceiver and assuming zero mean, Gaussian noise from each receiver.

δ{circumflex over (x)}_(tank)=(H _(tank) ^(T) V _(tank) ⁻¹ H _(tank))H_(tank) ^(T) V _(tank) ⁻¹({tilde over (ρ)}_(tank)− ρ _(tank))  (Eq. 93)

δ{circumflex over (x)}_(rec)=(H _(rec) ^(T) V _(rec) ⁻¹ H _(rec))H_(rec) ^(T) V _(rec) ⁻¹({tilde over (ρ)}_(rec)− ρ _(rec))  (Eq. 94)

The state estimates are then updated according to the following:

{circumflex over (x)} _(tank) = x _(tank) +δ{circumflex over (x)}_(tank)  (Eq. 95)

{circumflex over (x)} _(rec) = x _(rec) +δ{circumflex over (x)}_(rec)  (Eq. 96)

From the state, a posteriori residual may be formed in which the faultsignal is now assumed to be present.

r={tilde over (ρ)}− ρ−Hδ{circumflex over (x)}−μ _(i) −v _(i)  (Eq. 97)

A projector P is formed such that PH=0. For the matrix P to exist, thenumber of available satellites must be greater than 4; the size of theperturbed state space. Using this projector and assuming a no faultcondition, then the residual may be modified as:

Pr=P({tilde over (ρ)}− ρ−Hδ{circumflex over (x)}−μ_(i) −v)=P({tilde over(ρ)}− ρ−μ_(i) −v)  (Eq. 98)

In sum, the effect of the estimation error has been annihilated, andassuming a reasonable first estimate ({tilde over (ρ)}− ρ≈0), then Eq.(98) becomes:

Pr=P(v+μ _(i))  (Eq. 99)

If no fault exists (μ_(i)=0), then the statistics of Eq. (99) are zeromean and Gaussian with covariance V. The projector P has a null spaceequal to the size of the state space δ{circumflex over (x)}. This may beremoved from the residual since it is not relevant. The projector may bedecomposed as:

$\begin{matrix}{P = {{\begin{bmatrix}U_{1} & U_{2}\end{bmatrix}\begin{bmatrix}I & 0 \\0 & 0\end{bmatrix}}\begin{bmatrix}V_{1} \\V_{2}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 100} \right)\end{matrix}$

The residual in Eq. (99) may be rewritten neglecting the portion of theresidual annihilated by the projector:

{tilde over (r)}=U ₁ ^(T) r=V ₁(v+μ _(i))  (Eq. 101)

The probability density function of the new residual {tilde over (r)} isdefined as:

$\begin{matrix}{{f_{\overset{\sim}{r}}\left( \overset{\sim}{r} \right)} = {\frac{1}{\left( {2\; \pi} \right)^{\frac{n}{2}}{\Lambda_{r}}^{\frac{1}{2}}}^{({{- \frac{1}{2}}{({{\overset{\sim}{r}}^{T}\Lambda_{r\;}^{- 1}\overset{\sim}{r}})}})}}} & \left( {{Eq}.\mspace{14mu} 102} \right)\end{matrix}$

This is the density function for a Gaussian random variable {tilde over(r)}=U₁ ^(T)r with zero mean and covariance Λ_(r)=V₁VV₁ ^(T). The valueof n is equal to the number of GPS measurements used in the residualminus the state space δx size. The density function is utilized to testthe estimates in the Multiple Hypothesis Shiryayev SequentialProbability Ratio Test (MHSSPRT). Those residuals conforming to theGaussian characteristics are not likely to have a fault. However, thoseresiduals that are consistently multiple standard deviations away fromthe mean value are likely to have a failure.

To detect a failure, the GPS satellites are ordered into subsets. Eachsubset includes all of the satellites except one. For each subset, thesatellite that is excluded is assumed to be faulted. A separate leastsquares solution is computed for each subset of satellites using Eq.(860 through Eq. (96). The residual and probability density functionsare then formed for each subset as in Eq. (99) through Eq. (102).Referring back to FIG. 3, the residuals are formed in residual generator300. These probability density functions are tested using, for example,a MHSSPRT method in residual processor 305. This test sequentiallyupdates the probability that each subset has a fault. If a satellitefailure occurs, then all of the subsets will show a non-zero-meanprobability density function except for the set that excluded thesatellite. When the probability estimate of the case that excludes thefaulted satellite reaches a set probability limit, a fault is declaredand the satellite is excluded from the possible sets before use in theGEKF implemented in fault tolerant estimator 315. The tests are resetwithout that satellite and the process resumes looking for additionalsatellite failures.

Differential GPS Fault Detection

The methodology just discussed may be used to estimate failures indifferential GPS measurements (DGPS). Calculating the relative staterequires the differencing of state and pseudorange quantities. Therelative state estimate becomes:

Δ x= x _(tank) − x _(rec)  (Eq. 103)

An a priori relative range may be calculated as:

Δ ρ= ρ _(tank)− ρ _(rec)  (Eq. 104)

A single differenced GPS measurement is the difference of two rangemeasurements from a common satellite. The receiver range measurement ofsatellite i is subtracted from the tanker measurement of satellite i toform the single difference range measurement. This measurementeliminated the common mode errors defined in Eq. (90) and is denoted as:

Δ{tilde over (ρ)}=( ρ _(tank) +H _(tank) δx _(tank) +v _(tank))−( ρ_(rec) +H _(rec) δx _(rec) +v _(rec))  (Eq. 105)

Since the distance between the tanker and receiver is small incomparison to the distance to the satellites when the receiver is incontact position, it may be assumed that H_(tank)≈H_(rec). A newrelative range error may thus be defined as

Δ{tilde over (ρ)}= ρ _(tank)− ρ _(rec) +H _(tank)(δx _(tank) −δx_(rec))+v _(tank) −v _(rec)=Δ ρ+H _(tank) Δδx+Δv  (Eq. 106)

The new, relative state estimate error Δδx is estimated using a weightedleast squares solution to Eq. (106)

Δδ{circumflex over (x)}=[(H _(tank) ^(T)(2ΔV ⁻¹)H _(tank))H _(tank)^(T)(2ΔV ⁻¹)](Δ{tilde over (ρ)}−Δ ρ)  (Eq. 107)

And the posteriori residual for testing in the MHSSPRT is described as:

r _(Δ)={tilde over (ρ)}− ρ−HΔδx−μ _(i) −Δv  (Eq. 108)

Note that the fault μ_(i) may be from either the tanker or receiver.This particular test does not isolate the fault to the tanker orreceiver, but it does identify that a fault exists in the relative rangeestimates. The absolute estimates tests using the residuals defined inEq. (97) are required to isolate a failure to either the tanker orreceiver. However, the differential residual defined in Eq. (108) is notsusceptible to common mode errors and the relative measurementcovariance ΔV is typically an order of magnitude smaller than theabsolute covariance V for each vehicle which must be increasedsubstantially in order to compensate for the unmodeled atmosphericeffects.

As discussed with regard to absolute GPS fault detection, a projector Pis chosen such that PH=0 and used to modify the differential residualand eliminate the effect of the unknown estimation error. The resultsfollow the previous development to form a new residual {tilde over(r)}_(Δ)=U₁r_(Δ)=V₁(Δv+μ_(i)). The probability density function of thisresidual assuming no fault may be defined by

$\begin{matrix}{{f_{{\overset{\sim}{r}}_{\Delta}}\left( {\overset{\sim}{r}}_{\Delta} \right)} = {\frac{1}{\left( {2\; \pi} \right)^{\frac{n}{2}}{\Lambda_{r\; \Delta}}^{\frac{1}{2}}}^{({{- \frac{1}{2}}{({{\overset{\sim}{r}}_{\Delta}^{T}\Lambda_{r\; \Delta}^{- 1}{\overset{\sim}{r}}_{\Delta}})}})}}} & \left( {{Eq}.\mspace{14mu} 109} \right)\end{matrix}$

As discussed with regard to absolute GPS measurements, residualgenerator 300 may generate the appropriate residuals having probabilitydensity functions as shown in Eq. (109). Residual processor 305 thenexamines whether the residuals remain within the density function orbecause the residuals provide insight into the health of the DGPSmeasurements. If the residuals remain within the density function, thenthe probability of a failure is small. If the residuals reside outsidethe density functions, then the probability of failure is more likely.Therefore, the double differenced measurements may be placed intosub-groups. Each subgroup utilizes all of the satellites except for one,with each subgroup choosing a different satellite measurement toexclude. The differential GPS residuals are then processed through theMHSSPRT that calculates the probability that each of the subgroups ishealthy while the others are faulted. If one residual moves to aprobability of one or to some pre-defined bound, then the satellite thatwas excluded from that subgroup is declared faulty and removed from usein the measurement set. New subgroups are formed from the remainingsatellites, and the process continues.

The same process may be used for double differenced carrier phasemeasurements that may be tested independently or with the DGPSmeasurements in order to form additional measurements. For doubledifferenced carrier phase measurements, the projector is different,however, and must be calculated based on the double differenced line ofsight matrix. The MHSSPRT method will now be discussed in more detail.

Multiple Hypothesis Shiryayev Sequential Probability Ratio Test

The Multiple Hypothesis Shiryayev Sequential Probability Ratio Test(MHSSPRT) is a hypothesis testing scheme that may be performed inresidual processor 305. The test is designed to detect changes in aresidual history in minimum time based on assumed statisticalproperties. Given a residual history and associated probability densityfunction for a set of hypotheses, the MHSSPRT calculates the probabilitythat each hypothesis is true relative to the other hypotheses. TheMHSSPRT assumes that a base condition exists which is healthy and thencalculates the probability of a change to a different hypothesisassuming a probability of failure at each time step.

In equation form, the probability that a hypothesis j is correct at time0 is given by an initial condition:

F_(0,j)=π_(j)  (Eq. 110)

The initial condition π_(j) is chosen as a design parameter. At eachmeasurement time k, there is a finite probability that a failure hasoccurred between the time of the current measurement k and the previoustime step, k−1. The propagation of the probability is:

$\begin{matrix}{\phi_{k,j} = {F_{k,j} + {{\overset{\sim}{p}}_{j}\left( {1 - {\sum\limits_{l = 0}^{N}F_{k,j}}} \right)}}} & \left( {{Eq}.\mspace{14mu} 111} \right)\end{matrix}$

The value of N is the total number of possible fault modes currentlyaddressed in the MHSSPRT. For example, if 7 satellites are in view, thenthere are at least 7 fault modes considered, one for each satellite. Thetotal health condition in which it is assumed that all satellites arehealthy may or may not be included as a design choice.

When measurements become available, the residuals and associated densityfunctions are calculated as previously described. The densities areupdated utilizing the following update formula.

$\begin{matrix}{F_{{k + 1},j} = \frac{\phi_{k,j}{f_{{\overset{\sim}{r}}_{k + 1}h_{j}}\left( {{\overset{\sim}{r}}_{k + 1}h_{j}} \right)}}{\sum\limits_{l = 0}^{N}{\phi_{k,j}{f_{{\overset{\sim}{r}}_{k + 1}h_{j}}\left( {{\overset{\sim}{r}}_{k + 1}h_{j}} \right)}}}} & \left( {{Eq}.\mspace{14mu} 112} \right)\end{matrix}$

After each update, the probability is tested against a value. If theprobability exceeds a threshold (such as 0.99999) then that probabilityis declared true. The assumed hypothesis is assumed to be accurate. Inthis case, the subset of GPS measurements utilized which reaches thisvalue is the set of measurements that is healthy since all other subsets(hypothesized healthy measurements) have a probability near zero,indicating that the satellite excluded by the first subset is unhealthy.In this way the MHSSPRT identifies the failure and isolates the failurefrom other possible failures.

EO Fault Detection

EO fault detection is similar to the differential GPS fault detectiontechniques presented previously. A least squares technique is presentedas a means of utilizing excess observability in the vision systems as ameans of self checking. Two different types of estimation are described.

Reference Point Least Squares Filter

Reference point estimation assumes that the EO system is capable ofidentifying fixed points on the receiver aircraft with known lever armsbetween the points. The result is that the EO system provides bearingsmeasurements (elevation and azimuth) in the camera frame to each of thereference points. Reference points may be active such as beacon systems,passive such as known paint markings, or inherent in the receiver designsuch as the refueling receptacle. The relative position in the cameraframe V from the camera to a reference point is given by:

$\begin{matrix}{r_{T,R_{i}}^{V} = {{\Delta \; P_{i}^{V}} = {\begin{bmatrix}{x_{R_{i}} - x_{T}} \\{y_{R_{i}} - y_{T}} \\{z_{R_{i}} - z_{T}}\end{bmatrix} = {\left( {\Delta \; P^{V}} \right) + {C_{B_{R}}^{V}L_{R_{INS},R_{i}}^{B_{R}}}}}}} & \left( {{Eq}.\mspace{14mu} 113} \right)\end{matrix}$

In this case, the relative position is calculated only in the visionsensor reference frame. The sensor frame is designed as a right handedCartesian frame with x through the lens of the camera looking outtowards the receiver, the y-axis is defined to starboard of the camera,and the z-axis points down. The axes are centered at the lens of thevision system. The rotation matrix C_(B) _(R) ^(V) defines the rotationfrom the receiver body frame to the tanker vision frame. The lever armL_(R) _(INS) _(,R) _(i) ^(R) ^(R) is the vector position in the receiverbody frame from the receiver INS (or other common reference point) tothe reference point i. This vector is assumed known a priori. Since noother instruments are involved, the lever arm from the tanker visionsystem to the GPS or IMU is ignored. This algorithm calculates theposition and attitude of the receiver relative to the vision system. Thevalues for x_(T), y_(T), and z_(T) form the origin of the referenceframe and may be set to zero for convenience. The error in the relativeposition is defined as the difference between the a priori estimate ofthe relative position and a perturbation:

ΔP ^(V) =Δ P ^(V) +δΔP ^(V)  (Eq. 114)

The error in the attitude is defined relative to the target body frameas:

C _(B) _(R) ^(V) =C _(B) _(R) ^(V)(I+2[δq _(R)×])→C _(V) ^(B) ^(R)=(I−2[q _(R)×])C _(V) ^(B) ^(R)   (Eq. 115)

The definition for e^(V), the error between the true and estimatedposition and attitude is

$\begin{matrix}\begin{matrix}{e_{i}^{V} = \begin{bmatrix}e_{x}^{V} \\e_{y}^{V} \\e_{z}^{V}\end{bmatrix}} \\{= {\begin{bmatrix}{x_{R_{i}} - x_{T}} \\{y_{R_{i}} - y_{T}} \\{z_{R_{i}} - z_{T}}\end{bmatrix} - \begin{bmatrix}{{\overset{\_}{x}}_{R_{i}} - {\overset{\_}{x}}_{T}} \\{{\overset{\_}{y}}_{R_{i}} - {\overset{\_}{y}}_{T}} \\{{\overset{\_}{z}}_{R_{i}} - {\overset{\_}{z}}_{T}}\end{bmatrix}}} \\{= {{\Delta \; P^{V}} - {\Delta \; {\overset{\_}{P}}^{V}} + {C_{B_{R}}^{V}L_{R_{INS},R_{i}}^{B_{R}}} - {C_{{\overset{\_}{B}}_{R}}^{V}L_{R_{INS},R_{i}}^{B_{R}}}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 116} \right)\end{matrix}$

Substituting Eq. (114) and Eq. (115) into Eq. (116) produces thefollowing equation in terms of the error perturbations:

e ^(V) Δ P ^(V) +δΔP ^(V) −Δ P ^(V) +C _(B) _(R) ^(V)(I+2[δq _(R)×])L_(R) _(INS) _(,R) _(i) ^(B) ^(R) −C _(B) _(R) ^(V) L _(R) _(INS) _(,R)_(i) ^(B) ^(R)

e ^(V) ΔδΔP ^(V)−2C _(B) _(R) ^(V) [L _(R) _(INS) _(,R) _(i) ^(B) ^(R)×]δq _(R)  (Eq. 117)

The measurement matrix H_(e) is defined as

H _(e) =[I−2C _(B) _(R) ^(V) [L _(R) _(INS) _(,R) _(i) ^(B) ^(R)×]]  (Eq. 118)

The state δx is now the reduced order state:

$\begin{matrix}{{\delta \; x} = \begin{bmatrix}{\delta \; \Delta \; P^{V}} \\{\delta \; q_{T}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 119} \right)\end{matrix}$

The error equation in terms of the Cartesian location is of a form:

e^(V)=H_(e)δx  (Eq. 120)

If the measurements are in terms of a vector position of the referencepoint location relative to the camera (such as in the case of phasespace measurements), then the residual error measurement becomes:

r _(V) ={tilde over (P)} ^(V) − P ^(V) =H _(e) δx+v _(V)  (Eq. 121)

In this case, the measurement {tilde over (P)}^(V) is the position ofthe reference point in the vision reference frame measured by the phasespace estimation system with additive noise v_(V).

However, if the vision or Electro-optical system produces anglemeasurements in the form of elevation and azimuth, then the measurementsare defined as:

$\begin{matrix}{\alpha_{i} = {\tan^{- 1}\left( \frac{y_{T} - y_{R_{i}}}{x_{T} - x_{R_{i}}} \right)}} & \left( {{Eq}.\mspace{14mu} 122} \right)\end{matrix}$and

$\begin{matrix}{\beta_{i} = {\tan^{- 1}\left( \frac{z_{T} - z_{R_{i}}}{x_{T} - x_{R_{i}}} \right)}} & \left( {{Eq}.\mspace{14mu} 123} \right)\end{matrix}$

The conversion of specialized angle measurements to Cartesian errors maybe performed using a Modified Gain Extended Kalman Filter as discussedpreviously. In this filter, a measurement matrix H_(V) is defined suchthat a residual of the angles for each reference point take the form:

$\begin{matrix}{r_{\alpha \; \beta} = {\begin{bmatrix}{{\overset{\sim}{\alpha}}_{i} - {\overset{\_}{\alpha}}_{i}} \\{{\overset{\sim}{\beta}}_{i} - {\overset{\_}{\beta}}_{i}}\end{bmatrix} = {{H_{V}H_{e}\delta \; x} + \begin{bmatrix}v_{\alpha} \\v_{\beta}\end{bmatrix}}}} & \left( {{Eq}.\mspace{14mu} 124} \right)\end{matrix}$

In addition, the residuals which have measurements in sphericalcoordinates have linear errors in the Cartesian coordinates. Themeasurement noise v_(α) and v_(β) are typically modeled as pixel noiseon the angles.

Using multiple reference points (multiple are required for fullobservability), it is possible to estimate the solution to Eq. (124)using the following least squares solution:

$\begin{matrix}\begin{matrix}{{\delta \; \hat{x}} = {\left( {H_{e}^{T}H_{V}^{T}V_{V}^{- 1}H_{V}H_{e}} \right)^{- 1}H_{e}^{T}H_{V}^{T}{V_{V}^{- 1}\left( \begin{bmatrix}{\overset{\sim}{\alpha} - \overset{\_}{\alpha}} \\{\overset{\sim}{\beta} - \overset{\_}{\beta}}\end{bmatrix} \right)}}} \\{= \begin{bmatrix}{\delta \; \Delta \; P^{V}} \\{\delta \; q_{B_{T}}}\end{bmatrix}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 125} \right)\end{matrix}$

The relative position Δ P ^(E) is corrected using

Δ{circumflex over (P)} ^(E) =Δ P ^(E) +Δδ{circumflex over (P)}  (Eq.126)

The relative attitude is corrected by first constructing the quaternioncorrection:

$\begin{matrix}{Q_{{\hat{B}}_{T}}^{{\overset{\_}{B}}_{T}} = \begin{bmatrix}\sqrt{1 - \left( {\delta \; q_{T}^{T}\delta \; q_{T}} \right)} \\{\delta \; q_{T}}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 127} \right)\end{matrix}$

A corrected quaternion may then be formed by rotating the a prioriquatemion by the corrected update quaternion:

Q_({circumflex over (B)}) _(T) ^(V)=Q _(B) _(T) ^(V)

Q_({circumflex over (B)}) _(T) ^(B) ^(T)   (Eq. 128)

At this point the least squares solution to the relative navigationsystem is defined. The extension to fault detection is to assume thatone of the reference points is invalid. A least squares fault detectionfilter as discussed above is then utilized to determine which referencepoint is invalid and exclude it from the estimation process.

Silhouette Matching Least Squares

The previous section assumed that the image processing algorithmidentified reference points and calculated the bearings measurements tothe vehicle. An alternative approach is to attempt to use silhouettematching algorithms in which the target is matched with a library ofsilhouettes of the target to estimate the range and orientation of theentire target or target area. The output of this type of algorithmprovides additional information including a relative range and relativeattitude of the target to the vision system. An additional score of howwell the target and silhouette match is also included as a noise factor.

The benefit of silhouette matching is that it inherently takes intoaccount the geometry of the lever arms over an area of the target andproduces range and attitude information in addition to bearingsmeasurements. The process may be repeated locally or globally for agiven target geometry. For instance, the silhouette used may match theentire target geometry or just a portion of the target such as anaircraft canopy or tail section. Through the use of multiple silhouettesand sub-silhouettes for a given target, multiple relative ranges andattitudes may be used to estimate the total target range and relativeattitude. However, a disadvantage of this type of algorithm is that eachmeasurement is complex because it consists of complete relative positionand attitude rather than just angles. The result is a highly correlatedvector measurement. The capability of matching the silhouette to theimage does not have a known, stochastic error model in terms of themeasurements presented. Characterizing the measurements becomes moredifficult.

A single silhouette measurement produces a complete state of thefollowing form:

$\begin{matrix}{{\overset{\sim}{y}}_{i} = \begin{bmatrix}{\overset{\sim}{p}}_{i} \\{\overset{\sim}{\alpha}}_{i} \\{\overset{\sim}{\beta}}_{i} \\Q_{B_{T}}^{V}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 129} \right)\end{matrix}$

where {tilde over (α)}_(i) and {tilde over (β)}_(i) have the same angledefinition as in Eq. (122) and Eq. (123). The new variable {tilde over(ρ)}_(i) represents the scalar range from the vision system to thetarget along the vector line defined by {tilde over (α)}_(i) and {tildeover (β)}_(i). The quaternion is the estimated attitude of the targetrelative to the camera system as previously defined as part of Eq.(128).

Note that the silhouette matching algorithm still reflects the estimateof the range, attitude, and bearings from the camera to a particularpoint on or within the target. This location may be different fromalgorithm to algorithm. Regardless, the silhouette reference point maybe different than the location of the target INS or other referencepoint. Therefore, the relative range from the camera to the target INSis still defined as:

$\begin{matrix}{r_{T,R_{i}}^{V} = {{\Delta \; P_{i}^{V}} = {\begin{bmatrix}{x_{R_{i}} - x_{T}} \\{y_{R_{i}} - y_{T}} \\{z_{R_{i}} - z_{T}}\end{bmatrix} = {\left( {\Delta \; P^{V}} \right) + {C_{B_{R}}^{V}L_{R_{INS},R_{i}}^{B_{R}}}}}}} & \left( {{Eq}.\mspace{14mu} 130} \right)\end{matrix}$

The error model measurement for the measurements of {tilde over (α)}_(i)and {tilde over (β)}_(i) have been previously defined. The definition ofthe range measurement in the camera coordinate frame is:

$\begin{matrix}\begin{matrix}{\rho_{i} = {{\Delta \; P_{i}^{V}}}} \\{= {\begin{bmatrix}{x_{R_{i}} - x_{T}} \\{y_{R_{i}} - y_{T}} \\{z_{R_{i}} - z_{T}}\end{bmatrix}}} \\{= \sqrt{\left( {x_{R_{i}} - x_{T}} \right)^{2} + \left( {y_{R_{i}} - y_{T}} \right)^{2} + \left( {z_{R_{i}} - z_{T}} \right)^{2}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 131} \right)\end{matrix}$

The a priori estimate of the range is determined from:

$\begin{matrix}\begin{matrix}{{\overset{\_}{\rho}}_{i} = {{\Delta \; {\overset{\_}{P}}_{i}^{V}}}} \\{= {\begin{bmatrix}{{\overset{\_}{x}}_{R_{i}} - {\overset{\_}{x}}_{T}} \\{{\overset{\_}{y}}_{R_{i}} - {\overset{\_}{y}}_{T}} \\{{\overset{\_}{z}}_{R_{i}} - {\overset{\_}{z}}_{T}}\end{bmatrix}}} \\{= \sqrt{\left( {{\overset{\_}{x}}_{R_{i}} - {\overset{\_}{x}}_{T}} \right)^{2} + \left( {{\overset{\_}{y}}_{R_{i}} - {\overset{\_}{y}}_{T}} \right)^{2} + \left( {{\overset{\_}{z}}_{R_{i}} - {\overset{\_}{z}}_{T}} \right)^{2}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 132} \right)\end{matrix}$

The range measurement is defined as truth plus an additive noise term:

{tilde over (ρ)}_(i)=ρ_(i) +v _(ρ)  (Eq. 133)

Taking a first order perturbation in ΔP^(V), the true relative positionis again defined relative to the a priori range as:

ΔP _(i) ^(V) =Δ P _(i) ^(V) +δΔP _(i) ^(V)  (Eq. 134)

Taking a first order perturbation around the a priori estimate of range,the true range is approximated by:

ρ_(i)= ρ _(i) +h _(ρi) δΔP _(i) ^(V)  (Eq. 135)

Where H_(ρ) is defined as:

$\begin{matrix}\begin{matrix}{H_{\rho \; i} = {\frac{\partial\rho_{i}}{{\partial\delta}\; \Delta \; P_{i}^{V}}_{\Delta \; {\overset{\_}{P}}_{i}^{V}}}} \\{= \left\lbrack {\frac{{\overset{\_}{x}}_{R_{i}} - {\overset{\_}{x}}_{T}}{{\Delta \; {\overset{\_}{P}}_{i}^{V}}}\mspace{14mu} \frac{{\overset{\_}{y}}_{R_{i}} - {\overset{\_}{y}}_{T}}{{\Delta \; {\overset{\_}{P}}_{i}^{V}}}\mspace{14mu} \frac{{\overset{\_}{z}}_{R_{i}} - {\overset{\_}{z}}_{T}}{{\Delta \; {\overset{\_}{P}}_{i}^{V}}}} \right\rbrack}\end{matrix} & \left( {{Eq}.\mspace{14mu} 136} \right)\end{matrix}$

The measurement is therefore

{tilde over (ρ)}_(i)= ρ _(i) +H _(ρi) δΔP _(i) ^(V) +v _(p)  (Eq. 137)

For the reduced order state space defined in Eq. (119), the equivalentmeasurement function is

$\begin{matrix}\begin{matrix}{{\overset{\sim}{\rho}}_{i} = {{\overset{\_}{\rho}}_{i} + {{H_{\rho \; i}\left\lbrack {I - {2\; {C_{{\overset{\_}{B}}_{R}}^{V}\left\lbrack {L_{R_{INS},R_{i}}^{B_{R}} \times} \right\rbrack}}} \right\rbrack}\begin{bmatrix}{\delta \; \Delta \; P^{V}} \\{\delta \; q}\end{bmatrix}} + v_{\rho}}} \\{= {{\overset{\_}{\rho}}_{i} + {H_{\rho \; i}H_{e}\delta \; x} + v_{\rho}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 138} \right)\end{matrix}$

The quaternion measurement poses a problem in that simply converting toEuler angles and differencing the measurement with the a priori estimateis unacceptable because it obscures the inherent order of operations ofthe angles requires to correctly define the orientation. Further, theangles are difficult to reconcile with the quatemion errors presentedearlier. However, using cosine rotation matrices, the quatemion error inthe measurement may be defined. The definition of the true cosinerotation matrix in terms of the a priori estimate is defined as:

C _(B) _(T) ^(V) =C _(B) _(T) ^(V)(I+2[δq×])  (Eq. 139)

A similar version exists for Euler angle errors, but not the differenceof the Euler angles. A measurement of Euler angles can be converted to acosine rotation matrix with an error in the rotation due to noise.

The measurement matrix as a function of truth becomes:

C _(B) _(T) ^(V) =C _(B) _(T) ^(V)(I+[v _(q)×])  (Eq. 140)

In this case, the noise v_(q) is assumed to be zero mean Gaussian noiseon the Euler angles and small in comparison to the angles values. Thevector v_(q) is defined as:

$\begin{matrix}{v_{q} = \begin{bmatrix}v_{\Phi} \\v_{\Theta} \\v_{\Psi}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 141} \right)\end{matrix}$

The covariance for v_(q) is defined as V_(q). Substituting Eq. (139)into Eq. (140) gives a nonlinear expression for the attitude error andnoise:

$\begin{matrix}{C_{{\overset{\sim}{B}}_{T}}^{V} = {\left. {{C_{{\overset{\_}{B}}_{T}}^{V}\left( {I + {2\left\lbrack {\delta \; q \times} \right\rbrack}} \right)}\left( {I + \left\lbrack {v_{q} \times} \right\rbrack} \right)}\rightarrow {{C_{{\overset{\sim}{B}}_{T}}^{V}V_{V}^{{\overset{\_}{B}}_{T}}} - I} \right. = {{2\left\lbrack {\delta \; q \times} \right\rbrack} + \left\lbrack {v_{q} \times} \right\rbrack + {{2\left\lbrack {\delta \; q \times} \right\rbrack}\left\lbrack {v_{q} \times} \right\rbrack}}}} & \left( {{Eq}.\mspace{14mu} 142} \right)\end{matrix}$

As can be seen in Eq. (142), the resulting nonlinear definition resultsin state dependent noise. Two options may be considered for handlingthis problem. The first is to ignore the cross coupling between theerror in the quatemion and the measurement noise. This is not anunreasonable choice since the a priori estimate of the attitude error iszero so should not affect steady state performance. In that case, it ispossible to write three equations for the measurement function as:

$\begin{matrix}{\left( {C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)_{32} = {{{2\; \delta \; q_{1}} + {v_{\Phi}\left( {C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)}_{13}} = {{{2\; \delta \; q_{2}} + {v_{\Theta}\left( {C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)}_{21}} = {{2\; \delta \; q_{3}} + v_{\Psi}}}}} & \left( {{Eq}.\mspace{14mu} 143} \right)\end{matrix}$

The notation (C _(B) _(T) ^(V)C_(V) ^(B) ^(T) )_(ij) indicates thei^(th) row and j^(th) column of the 3×3 rotation matrix. The use of Eq.(143) is valid provided that the state dependent noise term in Eq. (142)is ignored. The resulting measurement residual to be processed in aleast squares filter is then:

$\begin{matrix}\begin{matrix}{r_{q} = \begin{bmatrix}\left( {C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)_{32} \\\left( {C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)_{13} \\\left( {C_{{\overset{\_}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} \right)_{21}\end{bmatrix}} \\{= {{\begin{bmatrix}0 & {2\; I}\end{bmatrix}\begin{bmatrix}{\delta \; \Delta \; P^{V}} \\{\delta \; q}\end{bmatrix}} + \begin{bmatrix}v_{\Phi} \\v_{\Theta} \\v_{\Psi}\end{bmatrix}}} \\{= {{H_{q}\delta \; x} + v_{q}}}\end{matrix} & \left( {{Eq}.\mspace{14mu} 144} \right)\end{matrix}$

The measurement matrix H_(q) is defined as:

H_(q)=[02I]  (Eq. 145)

Six unknowns thus result. However, note that the rotation matrix in Eq.(142) provides nine equations which may be used to solve for the sixunknowns. The set of equations is:

$\begin{matrix}{{{C_{{\overset{\sim}{B}}_{T}}^{V}C_{V}^{{\overset{\_}{B}}_{T}}} - I} = {\quad\left\lbrack \begin{matrix}{{{- v_{\Psi}}\delta \; q_{3}} - {v_{\Theta}\delta \; q_{2}}} & {{{- \delta}\; q_{3}} - v_{\Psi} + {v_{\Phi}\delta \; q_{2}}} & {{\delta \; q_{2}} + v_{\Theta} + {v_{\Phi}\delta \; q_{3}}} \\{{\delta \; q_{3}} + v_{\Psi} + {v_{\Theta}\delta \; q_{1}}} & {{{- v_{\Psi}}\delta \; q_{3}} - {v_{\Phi}\delta \; q_{1}}} & {{{- \delta}\; q_{1}} - v_{\Phi} + {v_{\Theta}\delta \; q_{3}}} \\{{{- \delta}\; q_{2}} - v_{\Theta} + {v_{\Psi}\delta \; q_{3}}} & {{\delta \; q_{1}} + v_{\Phi} + {v_{\Psi}\delta \; q_{2}}} & {{{- v_{\Theta}}\delta \; q_{2}} - {v_{\Phi}\delta \; q_{1}}}\end{matrix} \right\rbrack}} & \left( {{Eq}.\mspace{14mu} 146} \right)\end{matrix}$

These could be solved explicitly. The noise vector may be correlatedwith the range and angular measurements defined previously which maymake the solution to Eq. (146) ambiguous with relationship to the othernoise factors. The total measurement equation is:

$\begin{matrix}{\begin{bmatrix}{\overset{\sim}{\rho}}_{i} \\{\overset{\sim}{\alpha}}_{i} \\{\overset{\sim}{\beta}}_{i} \\Q_{B_{\overset{\_}{T}}}^{V}\end{bmatrix} = {\begin{bmatrix}{\overset{\_}{\rho}}_{i} \\{\overset{\_}{\alpha}}_{i} \\{\overset{\_}{\beta}}_{i} \\Q_{B_{\overset{\_}{T}}}^{V}\end{bmatrix} + {\begin{bmatrix}H_{\rho \; i} \\H_{V\; \alpha} \\H_{V\; \beta} \\H_{q}\end{bmatrix}{H_{e}\begin{bmatrix}{\delta \; \Delta \; P^{V}} \\{\delta \; q}\end{bmatrix}}} + \begin{bmatrix}v_{\rho} \\v_{\alpha} \\v_{\beta} \\v_{q}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 147} \right)\end{matrix}$

Using this set of measurement models, a least squares filter may becalculated that is similar to Eq. (125) so as to estimate the stateusing silhouette matching.

EO Fault Detection

A methodology for utilizing the vision system to detect failures in themeasurements is presented. The methodology takes advantage of the leastsquares estimator structure presented previously. In this case, it isassumed that one of the reference points is invalid and the goal is tofind the invalid measurement. The cause of the measurement is unknown aswell as the signal.

Given a set of possibly nonlinear measurements of the form:

{tilde over (y)} _(i) =f(x)+v _(i)  (Eq. 148)

where the measurement {tilde over (y)}i is a nonlinear function of thestate x and additive noise v_(i), then the generalized least squaresfault detection problem is of the form:

{tilde over (y)} _(i) = y _(i) +H _(i) δx+v _(i)+μ_(i)  (Eq. 149)

In this case, the measurement {tilde over (y)}_(i) is the ithmeasurement, the a priori estimate of the measurement is y _(i)=f( x),the associated state error δx is the first order perturbation of theerror in y _(i) with the measurement matrix H_(i) defined as:

$\begin{matrix}{H_{i} = \left. \frac{\partial y}{\partial x} \right|_{\overset{\_}{x}}} & \left( {{Eq}.\mspace{14mu} 150} \right)\end{matrix}$

The state x is the a priori estimate and the measurement matrix islinearized around this estimate. The measurement is corrupted by noisev_(i) and an unknown fault μ_(i) may exist in the ith measurement. It isassumed that with N measurements, only one fault exists.

Given the N measurements and no fault, the optimal least squaressolution to the problem in Eq. (149) is given by:

δ{circumflex over (x)}=(H ^(T) V ⁻¹ H)⁻¹ H ^(T) V ⁻¹({tilde over (y)}− y)  (Eq. 151)

In this case, the noise matrix V represents the covariance matrix of allof the measurements. The state is updated as

{circumflex over (x)}= x+δ{circumflex over (x)}  (Eq. 152)

The residual is the difference between the measurement and the a prioriestimate. If a fault occurs, then the residual has takes the form:

r=y− y=Hδx+v+μ  (Eq. 1⁵³)

In this case, μ is considered to be a column vector of zeros except forthe fault signal μ_(i) on the ith row representing a fault in the ithmeasurement.

An annihilator may be constructed such that the effect of the error inthe state estimate is annihilated from the residual. A commonannihilator is:

D=I−H(H ^(T) H)⁻¹ H ^(T)  (Eq. 154)

The annihilator is designed such that DH=0. The existence of theannihilator assumes that (H^(T)H)⁻¹ exists. This condition essentiallyrequires that the state is fully observable by the set of measurements.It also implies that the measurement matrix H has full column rank.Using the annihilator on the residual produces:

Dr=D({tilde over (y)}− y )=D(Hδx+v+μ)=D(v+μ)  (Eq. 155)

To be an effective fault detection method, more measurements than stateerrors should be present.

In other words, the measurement matrix H has full column rank and morerows than columns. If this is the case, then the annihilator may bedecomposed using a singular value decomposition of the form:

$\begin{matrix}{D = {{\begin{bmatrix}U_{1} & U_{2}\end{bmatrix}\begin{bmatrix}I & 0 \\0 & 0\end{bmatrix}}\begin{bmatrix}V_{1} \\V_{2}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 156} \right)\end{matrix}$

The identity matrix I is a square matrix of dimension equal to the statevector. Using this generalized form, a new residual may be constructedfrom the old one

{tilde over (r)}=U ₁ ^(T) r=V(v+μ)  (Eq. 157)

If it is assumed that the noise v is a zero mean, Gaussian distributedrandom variable with covariance V, then the new residual has a Gaussianprobability density function (assuming no fault) which is given by:

$\begin{matrix}{{f_{\overset{\sim}{r}}\left( \overset{\sim}{r} \right)} = {\frac{1}{\left( {2\; \pi} \right)^{\frac{n}{2}}{\Lambda_{r}}^{\frac{1}{2}}}^{({{- \frac{1}{2}}{({{\overset{\sim}{r}}^{T}\Lambda_{r}^{- 1}\overset{\sim}{r}})}})}}} & \left( {{Eq}.\mspace{14mu} 158} \right)\end{matrix}$

Referring back to FIG. 3, residual generator 300 generates the residualshaving the probability densities shown in Eq. (158). For thisrelationship, the covariance is given by:

Λ_(r) =V ₁(V)⁻¹ V ₁  (Eq. 159)

The value of n is the dimension of r.

If a fault is present, then the residual statistical properties of theresidual will not follow Eq. (158) so that a means of testing theresidual set against a probability ratio test such as the MHSSPRTdiscussed previously is formed. Such a test may be carried out inresidual processor 305 of FIG. 3.

For more than one fault, the measurement sets are subdivided intosubsets. Each subset contains all of the measurements except one. Eachsubset is utilized to compute a separate estimate of the state. Theresidual is then tested using, for example, a MHSSPRT method. If a faultoccurs in one measurement, then all of the subsets which incorporatethat measurement will be affected and the respective statisticalproperties of the residuals will no longer be zero mean Gaussian.However, the subset without the fault will remain zero mean Gaussianeffectively identifying the faulty measurement. The measurement is thenexcluded from the measurement set and new subsets are created for theremaining measurements until too few measurements remain to provide anyanalytic redundancy. A subset may be created to test the faultymeasurement and reintroduce it when the fault disappears, detected whenthe residual process is again zero mean and Gaussian.

INS Fault Detection

Detecting failures in the INS is more subtle than detecting faults inthe GPS and EO measurements. The fundamental measurements of an INS areangular velocity and acceleration in the vehicle's local body framewhereas the EO and GPS each measure relative position in a commoncoordinate frame. The EO system operates in the tanker camera frame andthe GPS system operates in the Earth Centered Earth Fixed referenceframe. Therefore, the only way to detect IMU instrument failures isthrough the dynamics of the inertial navigation system. This may be doneseparately for each IMU or through the GEKF.

INS Fault Detection through the GEKF

The GEKF may be extended to include INS faults. To do so, Eq. (1.1.7) ismodified as follows:

δ{dot over (x)}=Aδx+Bw+fμ  (Eq. 160)

The error in the navigation state defined as:

$\begin{matrix}{{\delta \; x} = \begin{bmatrix}{\delta \; P^{E}} \\{\delta \; V^{E}} \\{\delta \; q} \\{{\delta \; b_{\omega}}\;} \\{\delta \; b_{f}} \\{c\; \delta \; \tau}\end{bmatrix}} & \left( {{Eq}.\mspace{14mu} 161} \right)\end{matrix}$

where the perturbed error δx consists of 18 states: a position errorδP^(E) in the ECEF frame, a velocity error δV^(E), an attitude error δq,a bias in the rate gyros δb_(ω), a bias in the accelerometers δb_(f),and a bias in the GPS clock measurements cδθ which includes bias, drift,and clock acceleration terms. The errors are influenced by the noise inthe inertial navigation system defined as Bw to be a zero mean Gaussianrandom variable with covariance W. A fault signal μ is introducedthrough a fault direction matrix F. A different fault matrix F isdefined for each fault type, whether it is an x, y, z accelerometer, oran x, y, z gyro. Once the fault matrix is defined a separate Kalmanfilter is defined for each fault type to be tested.

The measurements take the form:

{tilde over (ρ)}= ρ+Hδx+v  (Eq. 162)

This model assumes that all of the GPS measurements are healthy. Themeasurement matrix H includes the lever arm distance from the IMU to theGPS antenna.

Discrete Time Fault Detection

A filter is designed to provide the best estimate of the state δx andthen block the effect of a failure μ on the state estimation process.The failure signal is not known, but the direction that the failureenters the system is defined by the matrix f in the dynamics of Eq.(160). The filter processes the GPS measurements in a means similar tothe Kalman filter for a single vehicle GPS/INS system. However, thefilter blocks the effect of the fault on the estimation process andprovides a fault-free residual process with which to test against otherfilters which may not be tuned to block the particular fault. A filterthat is not tuned to block the failure will exhibit non-Gaussianbehavior while a filter tuned to block a particular failure will have aresidual that remains Gaussian in the presence of the failure.

The discrete time fault detection filter of the form presented in Eq.(160) with measurements defined in Eq. (162) takes the followingrecursive form. First the dynamics and fault model to a discrete timeform. The following approximations may be utilized for a generic timestep Δt:

$\begin{matrix}{\Phi = {^{A\; \Delta \; t} \approx {I + {A\; \Delta \; t}}}} & \left( {{Eq}.\mspace{14mu} 163} \right) \\{\Gamma = {\left( {{I\; \Delta \; t} + {\frac{1}{2}A\; \Delta \; t^{2}}} \right)B}} & \left( {{Eq}.\mspace{14mu} 164} \right) \\{F = {\left( {{I\; \Delta \; t} + {\frac{1}{2}A\; \Delta \; t^{2}}} \right)f}} & \left( {{Eq}.\mspace{14mu} 165} \right)\end{matrix}$

The new dynamics are then:

δx(t+Δt)=Φδx(t)+Γw+Fμ  (Eq. 166)

The state update thus becomes

δ{circumflex over (x)}=δ x+K({tilde over (ρ)}− ρ−Hδ x )  (Eq. 167)

The gain K is selected in order to minimize the following cost function:

$\begin{matrix}{{\max\limits_{\mu}{\max\limits_{x{(0)}}{\frac{1}{2}{\sum\limits_{0}^{k\; \Delta \; t}\left( {{{\overset{\sim}{\rho} - \overset{\_}{\rho} - {H\; \delta \; \overset{\_}{x}}}}_{V^{- 1}} - {\mu }_{Q_{F}}^{2} - {{{\delta \; x} - {\delta \; \overset{\_}{x}}}}_{Q_{s}}} \right)}}}} - {\frac{1}{2}{{{x(0)} - {\delta \; {\overset{\_}{x}(0)}}}}_{\prod_{0}}}} & \left( {{Eq}.\mspace{14mu} 168} \right)\end{matrix}$

The gain K may thus be constructed using the fault detection updatedefined as:

K=πH ^(T)(R+HπH ^(T))⁻¹  (Eq. 169)

The value for R is a weighted version of the measurement noisecovariance and is defined as

R=V ⁻¹ −HQ _(s) H ^(T)  (Eq. 170)

The matrix Q_(s) is a design parameter matrix designed to improve filterperformance. Values must be chosen carefully in order to maintainpositive definiteness. The a priori covariance π is updated with themeasurements using the following update formula:

M=π−πH ^(T)(R+HπH ^(T))⁻¹ Hπ  (Eq. 171)

The state error is propagated forward in time using Eq. (166), unlessthe correction is applied to the state of the vehicle in which case thestate error is zeroed after each correction. The covariance ispropagated forward in time using:

$\begin{matrix}{{\Pi \left( {t + {\Delta \; t}} \right)} = {{\Phi \; M\; \Phi^{T}} + {\frac{1}{\gamma}{FQ}_{F}F^{T}} + {\Gamma \; W\; \Gamma^{T}}}} & \left( {{Eq}.\mspace{14mu} 172} \right)\end{matrix}$

This propagation is similar to a standard Kalman filter with thecovariance modified by the discrete time dynamics Φ with additiveprocess noise W. The additional term essentially “blocks” the faultdirection F in the covariance, increasing the uncertainty in thatdirection by a weighting factor Q_(F) in order to force the filter toaccept that there is larger uncertainty in that direction due to thepossible existence of a failure. The weighting term γ is sometimes usedas a design parameter to maintain reasonable filter performance, but isset to one for these sets of experiments.

Using this filter, the residual may be tested for failures. Theposteriori residual is formed as:

{circumflex over (r)}={tilde over (ρ)}−{circumflex over (ρ)}  (Eq. 173)

A projector P is constructed to annihilate the effect of the stateestimation error on the measurement residual, (PH=0). The annihilatorhas a null space associated with the fault direction. The matrix P maybe decomposed as with other filter structures presented into thefollowing form:

$\begin{matrix}{P = {{\begin{bmatrix}U_{1} & U_{2}\end{bmatrix}\begin{bmatrix}I & 0 \\0 & 0\end{bmatrix}}\begin{bmatrix}V_{1} \\V_{2}\end{bmatrix}}} & \left( {{Eq}.\mspace{14mu} 174} \right)\end{matrix}$

The identity matrix I is a square matrix of dimension equal to the statevector. Using this generalized form, then a new residual may beconstructed from the old one

{tilde over (r)}=U ₁ ^(T) {circumflex over (r)}=V ₁(v+HFμ)  (Eq. 175)

If it is assumed that the measurement noise v is a zero mean, Gaussiandistributed random variable with covariance V, then the new residual hasa Gaussian probability density function (assuming no fault) which isgiven by:

$\begin{matrix}{{f_{\overset{\sim}{r}}\left( \overset{\sim}{r} \right)} = {\frac{1}{\left( {2\; \pi} \right)^{\frac{n}{2}}{\Lambda_{r}}^{\frac{1}{2}}}^{({{- \frac{1}{2}}{({{\overset{\sim}{r}}^{T}\Lambda_{r}^{- 1}\overset{\sim}{r}})}})}}} & \left( {{Eq}.\mspace{14mu} 176} \right)\end{matrix}$

In this case the covariance is given by:

Λ_(r) =V ₁(V)⁻¹ V ₁  (Eq. 177)

The value of n is the dimension of {tilde over (r)}.

As discussed previously, the residual may be tested using the MHSSPRTmethod defined previously. A filter designed to block a particularfailure will have a residual that remains zero mean and Gaussian even inthe presence of a failure. However, other failures will exhibitnon-Gaussian behavior indicating that a failure has occurred. The filterdesigned to block the failure remains healthy and the operationcontinues without interruption while other filters are discarded ashaving processed faulty data. The residual is used to detect and isolatethe failure and the filter designed to block the failure is utilized tocontinue the estimation process in the even of the failure.

Therefore, a bank of fault detection filters 300 may be used as shown inFIG. 3. Each filter is tuned to block one of the accelerometer axes orrate gyro axes. Six filters are required in addition to the healthyfilter which assumes no fault. When a failure occurs all of the filterswill show non-Gaussian statistics except for the filter tuned to blockthe failure. This filter will remain healthy despite the failure and canbe used to continue estimation.

As seen in FIG. 3, a fault reconstruction process 310 identifies thefault magnitude introduced by, for example, a particular accelerometerof rate gyroscope. Fault tolerant estimator 315 would thus comprise aGEKF that excludes measurements from the faulty sensor. In this fashion,an automated control of a refueling boom is enabled despite IMU faults,GPS faults, EO faults, or faults in other sensors such as RF rangingsensors.

It will be obvious to those skilled in the art that various changes andmodifications may be made without departing from this invention in itsbroader aspects. The appended claims encompass all such changes andmodifications as fall within the true spirit and scope of thisinvention.

1. A system for automated control of a refueling boom coupled to atanker aircraft, comprising: a first inertial measurement unit (IMU)providing inertial measurements for the tanker aircraft; a first GPSreceiver providing a GPS location for a GPS antenna attached to thetanker aircraft; and a processor adapted to calculate a first inertialnavigation state for the tanker aircraft through integration of theinertial measurements, the processor being further adapted to calculatea first inertial navigation state error relative to the GPS location andto filter the first inertial navigation state error and the firstinertial navigation state based upon noise characteristics of the firstIMU and the first GPS receiver to provide an updated inertial navigationstate for the tanker aircraft, the processor being further adapted tocontrol the refueling boom relative to a receiver aircraft based uponthe first and updated inertial navigation states.
 2. The system of claim1, wherein the processor is adapted to calculate the first inertialnavigation error by translating an IMU location from the first inertialnavigation state to the GPS location.
 3. The system of claim 1, whereinthe processor is adapted to filter the first inertial navigation stateusing a Kalman filter.
 4. The system of claim 3, wherein the Kalmanfilter is an extended Kalman filter.
 5. The system of claim 1, furthercomprising: a second IMU providing inertial measurements the receiveraircraft; and a second GPS receiver providing a GPS location for a GPSantenna attached to the receiver aircraft; wherein the processor isfurther adapted to integrate the inertial measurements from the secondIMU to provide a second inertial navigation state for the receiveraircraft, and wherein the processor is further adapted to calculate asecond inertial navigation state error relative to the GPS location forthe second GPS receiver and to filter the second inertial navigationstate error and the second inertial navigation state based upon noisecharacteristics of the second IMU and the second GPS receiver to providean updated inertial navigation state for the receiver aircraft, theprocessor being further adapted to control the refueling boom relativeto a receiver aircraft based upon the first inertial navigation state,the second inertial navigation state, and the updated inertialnavigation states.
 6. The system of claim 5, wherein the processor isadapted to calculate the second inertial navigation error by translatingan IMU location on the receiver aircraft from the second inertialnavigation state to the GPS location for the GPS antenna on the receiveraircraft.
 7. The system of claim 6, further comprising an electro-optic(EO) sensor adapted to image a plurality of reference points on thereceiver aircraft, wherein the processor is further adapted to controlthe refueling boom relative to errors determined through comparison of(EO) sensor-derived locations for the reference points tosecond-inertial-navigation-state-derived locations for the referencepoints.
 8. The system of claim 5, wherein the processor comprises atleast a first processor on the tanker aircraft and a second processor onthe receiver aircraft.
 9. The system of claim 8, further comprising awireless link between the receiver aircraft and the tanker aircraftadapted to permit the first and second processors to exchange positionalinformation to aid the control of the refueling boom.
 10. The system ofclaim 5, further comprising: an RF ranging sensor adapted to measure arange between the receiver aircraft and the tanker aircraft, wherein theprocessor is further adapted to control the refueling boom based uponthe measured range.
 11. A system for control of a refueling boom coupledto a tanker aircraft, comprising: an IMU providing tanker inertialmeasurements for the tanker aircraft; an IMU providing receiver inertialmeasurement for a receiver aircraft; and means for controlling therefueling boom so as to automatically mate with the receiver aircraft,wherein the means is adapted to control the refueling boom by filteringthe tanker and receiver inertial measurements relative to measurementsfrom a sensor selected from the group consisting of a GPS receiver andan EO sensor.
 12. The system of claim 11, wherein the sensor is a tankeraircraft GPS receiver and a receiver aircraft GPS receiver.
 13. Thesystem of claim 12, wherein the means is adapted to filter the tankerand receiver inertial measurements through integration of the tanker andreceiver inertial measurements to provide tanker and receiver inertialnavigation states.
 14. The system of claim 13, wherein the means isfurther adapted to calculate tanker and receiver inertial navigationerrors through comparison of the tanker and receiver inertialnavigations states to positions derived from the tanker aircraft GPSreceiver and the receiver aircraft GPS receiver.
 15. A method ofcontrolling a refueling boom relative to a tanker aircraft and areceiver aircraft, comprising: deriving an inertial navigation state forthe tanker aircraft; deriving an inertial navigation state for thereceiver aircraft; comparing the inertial navigation states toadditional sensor measurements to derive an inertial navigation stateerror for the tanker aircraft and an inertial navigation state error forthe receiver aircraft; filtering the inertial navigation states and theinertial navigation state errors to provide an updated inertialnavigation state for the receiver aircraft and an updated inertialnavigation state for the tanker aircraft; and controlling the refuelingboom based upon the updated inertial navigations states so as toautomatically mate with the receiver aircraft.
 16. The method of claim15, wherein filtering the inertial navigation state error comprisesfiltering the inertial navigation state error for the tanker aircraftand a relative navigation state error that equals a difference betweenthe inertial navigation state error for the tanker aircraft and theinertial navigation state error for the receiver aircraft.
 17. Themethod of claim 16, wherein the filtering comprising a Kalman filteringof the inertial navigation states, the inertial navigation state for thetanker aircraft, and the relative navigation state error.
 18. The methodof claim 17, wherein the Kalman filtering comprises an extended Kalmanfiltering.
 19. The method of claim 15, wherein deriving the inertialnavigation states for the receiver and tanker aircrafts comprisingintegrating inertial measurements from inertial measurement units. 20.The method of claim 19, wherein integrating the inertial measurementscomprises a non-linear integrating of the inertial measurements.